Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10036] pkg/trunk
|
From: <ge...@us...> - 2009-01-23 02:30:40
|
Revision: 10036
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10036&view=rev
Author: gerkey
Date: 2009-01-23 02:30:34 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Added /global_frame_id parameter, which is retrieved by nav_view and move_base
to set the "global" frame. By default, it's "map," but it can now be
changed. Required changes in observation buffers to pass down the frame.
Modified Paths:
--------------
pkg/trunk/highlevel/executive_python/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h
pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Added Paths:
-----------
pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
pkg/trunk/demos/2dnav_stage/maps/willow-empty-0.05.pgm
pkg/trunk/demos/2dnav_stage/worlds/willow-gps-pr2-5cm.world
Added: pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch (rev 0)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch 2009-01-23 02:30:34 UTC (rev 10036)
@@ -0,0 +1,63 @@
+<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="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">
+ <remap from="scan" to="base_scan" />
+ </node -->
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
+ <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> -->
+
+ <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/planner_frequency" value="0.0"/>
+ <param name="move_base/plannerTimeLimit" value="0.5"/>
+ <param name="move_base/map_update_frequency" value="5.0"/>
+ <param name="/costmap_2d/base_laser_max_range" value="20.0"/>
+ <param name="/costmap_2d/tilt_laser_max_range" value="2.0"/>
+ <param name="/costmap_2d/lethal_obstacle_threshold" value="100.0"/>
+ <param name="/costmap_2d/no_information_value" value="255"/>
+ <param name="/costmap_2d/z_threshold_max" value="2.0"/>
+ <param name="/costmap_2d/z_threshold_min" value="0.13"/>
+ <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="/global_frame_id" value="odom"/>
+
+ <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 10 Hz -->
+ <param name="/costmap_2d/base_laser_update_rate" value="2.0"/>
+
+ <!-- Don't Forces a check that we are receiving tilt scans that are correctly up to date at a rate of at least 10 Hz -->
+ <param name="/costmap_2d/tilt_laser_update_rate" value="0.0"/>
+
+ <!-- we don't have stereo data yet -->
+ <param name="/costmap_2d/stereo_update_rate" value="0.0"/>
+
+ <!--- Don't check for ground plane obstacles -->
+ <param name="/costmap_2d/low_obstacle_update_rate" value="0.0"/>
+
+ <param name="/trajectory_rollout/map_size" value="4.0" />
+ <param name="/trajectory_rollout/sim_time" value="1.0" />
+ <param name="/trajectory_rollout/sim_steps" value="20" />
+ <param name="/trajectory_rollout/samples_per_dim" value="25" />
+ <param name="/trajectory_rollout/occdist_scale" value="0.2" />
+ <param name="/trajectory_rollout/path_distance_bias" value="0.6" />
+ <param name="/trajectory_rollout/goal_distance_bias" value="0.8" />
+ <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" />
+
+ <node pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen"/>
+
+ </group>
+</launch>
+
Added: pkg/trunk/demos/2dnav_stage/maps/willow-empty-0.05.pgm
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/demos/2dnav_stage/maps/willow-empty-0.05.pgm
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: pkg/trunk/demos/2dnav_stage/worlds/willow-gps-pr2-5cm.world
===================================================================
--- pkg/trunk/demos/2dnav_stage/worlds/willow-gps-pr2-5cm.world (rev 0)
+++ pkg/trunk/demos/2dnav_stage/worlds/willow-gps-pr2-5cm.world 2009-01-23 02:30:34 UTC (rev 10036)
@@ -0,0 +1,71 @@
+define block model
+(
+ size3 [0.5 0.5 0.75]
+ gui_nose 0
+)
+
+define topurg laser
+(
+ range_min 0.0
+ range_max 30.0
+ fov 270.25
+ samples 1081
+ # generic model properties
+ color "black"
+ size [ 0.05 0.05 0.1 ]
+)
+
+define pr2 position
+(
+ size3 [0.65 0.65 0.25]
+ origin3 [-0.05 0 0 0]
+ gui_nose 1
+ drive "omni"
+ localization "gps"
+ localization_origin [3 3 90]
+ topurg(pose [0.275 0.000 0.000])
+)
+
+define floorplan model
+(
+ # sombre, sensible, artistic
+ color "gray30"
+
+ # most maps will need a bounding box
+ boundary 1
+
+ gui_nose 0
+ gui_grid 0
+ gui_movemask 0
+ gui_outline 0
+ gripper_return 0
+ fiducial_return 0
+ laser_return 1
+)
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.02
+
+interval_sim 100 # simulation timestep in milliseconds
+interval_real 100 # real-time interval between simulation updates in milliseconds
+
+window
+(
+ size [ 745.000 448.000 ]
+ center [275.990 494.960]
+ rotate [ 0.000 -1.560 ]
+ scale 18.806
+)
+
+# load an environment bitmap
+floorplan
+(
+ name "willow"
+ bitmap "../maps/willow-full-0.05.pgm"
+ size3 [58.25 47.25 1.0]
+ pose [-23.625 29.125 90.000]
+)
+
+# throw in a robot
+pr2( pose [-28.610 13.562 99.786] name "pr2" color "blue")
+block( pose [-25.062 12.909 180.000] color "red")
Modified: pkg/trunk/highlevel/executive_python/manifest.xml
===================================================================
--- pkg/trunk/highlevel/executive_python/manifest.xml 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/highlevel/executive_python/manifest.xml 2009-01-23 02:30:34 UTC (rev 10036)
@@ -5,6 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="rospy" />
<depend package="highlevel_controllers" />
+ <depend package="executive_trex_pr2" />
<depend package="std_msgs" />
<depend package="robot_msgs" />
</package>
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-01-23 02:30:34 UTC (rev 10036)
@@ -225,6 +225,8 @@
tf::TransformListener tf_; /**< Used to do transforms */
+ std::string global_frame_; /**< Which is our global frame? Usually "map" */
+
// Observation Buffers are dynamically allocated since their constructors take
// arguments bound by lookup to the param server. This could be chnaged with some reworking of how paramaters
// are looked up. If we wanted to generalize this node further, we could use a factory pattern to dynamically
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-01-23 02:30:34 UTC (rev 10036)
@@ -89,6 +89,8 @@
double circumscribedRadius(0.46);
double inscribedRadius(0.325);
double weight(0.1); // Scale costs down by a factor of 10
+ // Which frame is "global"
+ param("/global_frame_id", global_frame_, std::string("map"));
param("/costmap_2d/base_laser_max_range", baseLaserMaxRange_, baseLaserMaxRange_);
param("/costmap_2d/tilt_laser_max_range", tiltLaserMaxRange_, tiltLaserMaxRange_);
@@ -160,19 +162,19 @@
}
// Then allocate observation buffers
- baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), tf_,
+ baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
ros::Duration().fromSec(base_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), tf_,
+ tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
ros::Duration().fromSec(tilt_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), tf_,
+ lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
ros::Duration().fromSec(low_obstacle_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
inscribedRadius, -10.0, maxZ_, filter_);
- stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), tf_,
+ stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
ros::Duration().fromSec(stereo_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
@@ -305,7 +307,7 @@
subscribe("base_scan", baseScanMsg_, &MoveBase::baseScanCallback, 1);
tiltLaserNotifier_ = new tf::MessageNotifier<std_msgs::PointCloud>(&tf_, this,
boost::bind(&MoveBase::tiltCloudCallback, this, _1),
- "tilt_laser_cloud_filtered", "map", 50);
+ "tilt_laser_cloud_filtered", global_frame_, 50);
subscribe("dcam/cloud", stereoCloudMsg_, &MoveBase::stereoCloudCallback, 1);
subscribe("ground_plane", groundPlaneMsg_, &MoveBase::groundPlaneCallback, 1);
subscribe("obstacle_cloud", groundPlaneCloudMsg_, &MoveBase::groundPlaneCloudCallback, 1);
@@ -353,7 +355,7 @@
robotPose.stamp_ = ros::Time();
try{
- tf_.transformPose("map", robotPose, global_pose_);
+ tf_.transformPose(global_frame_, robotPose, global_pose_);
}
catch(tf::LookupException& ex) {
ROS_INFO("No Transform available Error\n");
@@ -391,7 +393,7 @@
goalPose.stamp_ = ros::Time();
try{
- tf_.transformPose("map", goalPose, transformedGoalPose);
+ tf_.transformPose(global_frame_, goalPose, transformedGoalPose);
}
catch(tf::LookupException& ex){
ROS_ERROR("No transform available from %s to map. This may be because the frame_id of the goalMsg is wrong.\n", goalMsg.header.frame_id.c_str());
@@ -743,7 +745,7 @@
target_point.point.y = pty;
target_point.point.z = 1;
target_point.header.stamp = ros::Time::now();
- target_point.header.frame_id = "map";
+ target_point.header.frame_id = global_frame_;
publish("head_controller/head_track_point", target_point);
return planOk;
}
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-01-23 02:30:34 UTC (rev 10036)
@@ -114,6 +114,8 @@
render_panel_->getViewport()->setCamera( camera_ );
+ ros_node_->param("/global_frame_id", global_frame_id_, std::string("map"));
+
ros_node_->advertise<robot_msgs::Planner2DGoal>("goal", 1);
ros_node_->advertise<std_msgs::Pose2DFloat32>("initialpose", 1);
ros_node_->subscribe("particlecloud", cloud_, &NavViewPanel::incomingParticleCloud, this, 1);
@@ -434,7 +436,7 @@
tf::Stamped<tf::Pose> robot_pose(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time(), "base_link");
tf::Stamped<tf::Pose> map_pose;
- tf_client_->transformPose("map", robot_pose, map_pose);
+ tf_client_->transformPose("odom", robot_pose, map_pose);
double yaw, pitch, roll;
map_pose.getBasis().getEulerZYX(yaw, pitch, roll);
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-01-23 02:30:34 UTC (rev 10036)
@@ -156,6 +156,7 @@
float getMapResolution() { return map_resolution_; }
int getMapWidth() { return map_width_; }
int getMapHeight() { return map_height_; }
+ const std::string& getGlobalFrame() { return global_frame_id_; }
protected:
@@ -234,6 +235,9 @@
float scale_;
+ // Global frame (usually "map")
+ std::string global_frame_id_;
+
bool new_cloud_;
bool new_gui_path_;
bool new_local_path_;
Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-01-23 02:30:34 UTC (rev 10036)
@@ -163,7 +163,7 @@
goal.goal.y = pos_.y;
goal.goal.th = angle;
goal.enable = 1;
- goal.header.frame_id = "map";
+ goal.header.frame_id = panel_->getGlobalFrame();
printf("setting goal: %.3f %.3f %.3f\n", goal.goal.x, goal.goal.y, goal.goal.th);
ros_node_->publish( "goal", goal );
}
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h 2009-01-23 02:30:34 UTC (rev 10036)
@@ -47,7 +47,7 @@
*/
class BasicObservationBuffer : public ObservationBuffer {
public:
- BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter = NULL);
+ BasicObservationBuffer(const std::string& frame_id, const std::string& global_frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter = NULL);
virtual void buffer_cloud(const std_msgs::PointCloud& local_cloud);
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h 2009-01-23 02:30:34 UTC (rev 10036)
@@ -47,7 +47,7 @@
*/
class ObservationBuffer {
public:
- ObservationBuffer(const std::string& frame_id, ros::Duration keep_alive, ros::Duration refresh_interval);
+ ObservationBuffer(const std::string& frame_id, const std::string& global_frame_id, ros::Duration keep_alive, ros::Duration refresh_interval);
virtual ~ObservationBuffer();
@@ -79,6 +79,8 @@
protected:
const std::string frame_id_;
+ /**< Which frame is global; usually "map" */
+ const std::string global_frame_id_;
private:
std::list<Observation> buffer_;
Modified: pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-23 02:30:34 UTC (rev 10036)
@@ -3,9 +3,9 @@
namespace costmap_2d {
- BasicObservationBuffer::BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,
+ BasicObservationBuffer::BasicObservationBuffer(const std::string& frame_id, const std::string& global_frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,
double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter)
- : ObservationBuffer(frame_id, keepAlive, refresh_interval), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ), filter_(filter) {}
+ : ObservationBuffer(frame_id, global_frame_id, keepAlive, refresh_interval), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ), filter_(filter) {}
void BasicObservationBuffer::buffer_cloud(const std_msgs::PointCloud& local_cloud)
{
@@ -35,12 +35,12 @@
{
// First we want the origin for the sensor
tf::Stamped<btVector3> local_origin(btVector3(0, 0, 0), point_cloud.header.stamp, frame_id_);
- tf_.transformPoint("map", local_origin, map_origin);
+ tf_.transformPoint(global_frame_id_, local_origin, map_origin);
tf_.transformPointCloud("base_footprint", point_cloud, base_cloud);
newData = extractFootprintAndGround(base_cloud);
map_cloud = new std_msgs::PointCloud();
- tf_.transformPointCloud("map", *newData, *map_cloud);
+ tf_.transformPointCloud(global_frame_id_, *newData, *map_cloud);
ROS_DEBUG("Buffering cloud for %s at origin <%f, %f, %f>\n", frame_id_.c_str(), map_origin.getX(), map_origin.getY(), map_origin.getZ());
}
Modified: pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-23 02:30:34 UTC (rev 10036)
@@ -41,8 +41,8 @@
return ros::Duration((int) rint(period), (int) rint((period - rint(period)) * pow(10.0, 9.0)));
}
- ObservationBuffer::ObservationBuffer(const std::string& frame_id, ros::Duration keep_alive, ros::Duration refresh_interval)
- :frame_id_(frame_id), received_obseration_(false), keep_alive_(keep_alive),
+ ObservationBuffer::ObservationBuffer(const std::string& frame_id, const std::string& global_frame_id, ros::Duration keep_alive, ros::Duration refresh_interval)
+ :frame_id_(frame_id), global_frame_id_(global_frame_id), received_obseration_(false), keep_alive_(keep_alive),
refresh_interval_(refresh_interval), last_updated_(ros::Time::now()) {
ROS_INFO("Initializing observation buffer for %s with keepAlive = %f and refresh_interval = %f\n",
frame_id_.c_str(), keep_alive.toSec(), refresh_interval_.toSec());
@@ -66,7 +66,7 @@
last_updated_ = ros::Time::now();
received_obseration_ = true;
- if(observation.cloud_->header.frame_id != "map") {
+ if(observation.cloud_->header.frame_id != global_frame_id_) {
ROS_ERROR("Observation in frame %s, must be in frame \"map\".",
observation.cloud_->header.frame_id.c_str());
return false;
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2009-01-23 02:30:34 UTC (rev 10036)
@@ -187,7 +187,7 @@
ros::Duration keep_alive(10, 0);
ros::Duration refresh_interval(0, 200000000); // 200 ms
- ObservationBuffer buffer("Foo", keep_alive, refresh_interval);
+ ObservationBuffer buffer("Foo", "map", keep_alive, refresh_interval);
// Initially it should be false
ASSERT_EQ(buffer.isCurrent(), false);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
