Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[5542] pkg/trunk
|
From: <ge...@us...> - 2008-10-19 17:58:06
|
Revision: 5542
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5542&view=rev
Author: gerkey
Date: 2008-10-19 17:57:59 +0000 (Sun, 19 Oct 2008)
Log Message:
-----------
Changed publication of 'scan' to 'base_scan' in rosstage, to match
rosgazebo and PR2. Update roslaunch files to match, remapping scan to
base_scan for amcl_player and wavefront_player.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_stage/2dnav_stage.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_5cm.xml
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage.xml 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage.xml 2008-10-19 17:57:59 UTC (rev 5542)
@@ -1,8 +1,8 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-erratic.world" respawn="false" output="screen"/>
- <node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full.pgm 0.1" respawn="false" output="screen"/>
+ <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/willow-erratic.world" respawn="false" output="screen"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/willow-full.pgm 0.1" respawn="false" output="screen"/>
<param name="pf_laser_max_beams" value="20"/>
<param name="pf_min_samples" value="500"/>
<param name="pf_max_samples" value="10000"/>
@@ -12,9 +12,14 @@
<param name="pf_odom_drift_xa" value="0.2"/>
<param name="pf_min_d" value="0.25"/>
<param name="pf_min_a" value="0.524"/>
- <node pkg="amcl_player" type="amcl_player" respawn="false" output="screen"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
+ <node pkg="amcl_player" type="amcl_player" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ </node>
+ <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"/> -->
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization.xml 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization.xml 2008-10-19 17:57:59 UTC (rev 5542)
@@ -3,7 +3,9 @@
<group name="wg">
<node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-pr2.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full.pgm 0.1" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" />
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" >
+ <remap from="scan" to="base_scan" />
+ </node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
<!-- TODO: remove this remap after fake_localization is fixed to listen
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_5cm.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_5cm.xml 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_5cm.xml 2008-10-19 17:57:59 UTC (rev 5542)
@@ -3,7 +3,9 @@
<group name="wg">
<node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-pr2-5cm.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full-0.05.pgm 0.05" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" />
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" >
+ <remap from="scan" to="base_scan" />
+ </node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
<!-- TODO: remove this remap after fake_localization is fixed to listen
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2008-10-19 17:57:59 UTC (rev 5542)
@@ -3,7 +3,7 @@
<author>Brian Gerkey</author>
<license>BSD</license>
<url>http://pr.willowgarage.com/wiki/FIXME</url>
- <depend package="nav_view"/>
+ <depend package="nav_view_sdl"/>
<depend package="roslaunch"/>
<depend package="amcl_player"/>
<depend package="fake_localization"/>
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-10-19 17:57:59 UTC (rev 5542)
@@ -62,7 +62,7 @@
Publishes to (name / type):
- @b "odom"/RobotBase2DOdom : odometry data from the position model.
-- @b "scan"/LaserScan : scans from the laser model.
+- @b "base_scan"/LaserScan : scans from the laser model.
<hr>
@@ -205,7 +205,7 @@
return(-1);
}
- advertise<std_msgs::LaserScan>("scan",10);
+ advertise<std_msgs::LaserScan>("base_scan",10);
advertise<std_msgs::RobotBase2DOdom>("odom",10);
advertise<std_msgs::TransformWithRateStamped>("base_pose_ground_truth",10);
subscribe("cmd_vel", velMsg, &StageNode::cmdvelReceived, 10);
@@ -226,6 +226,9 @@
// Let the simulator update (it will sleep if there's time)
this->world->Update();
+ ros::Time sim_time;
+ sim_time.fromSec(world->SimTimeNow() / 1e6);
+
// Get latest laser data
Stg::stg_laser_sample_t* samples = this->lasermodel->GetSamples();
if(samples)
@@ -235,25 +238,33 @@
this->laserMsg.angle_min = -cfg.fov/2.0;
this->laserMsg.angle_max = +cfg.fov/2.0;
this->laserMsg.angle_increment = cfg.fov / (double)(cfg.sample_count-1);
+ this->laserMsg.range_min = 0.0;
this->laserMsg.range_max = cfg.range_bounds.max;
- this->laserMsg.set_ranges_size(cfg.sample_count);
- this->laserMsg.set_intensities_size(cfg.sample_count);
+ this->laserMsg.ranges.resize(cfg.sample_count);
+ this->laserMsg.intensities.resize(cfg.sample_count);
for(unsigned int i=0;i<cfg.sample_count;i++)
{
this->laserMsg.ranges[i] = samples[i].range;
this->laserMsg.intensities[i] = (uint8_t)samples[i].reflectance;
}
- // TODO: get the frame ID from somewhere
this->laserMsg.header.frame_id = "base_laser";
- this->laserMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
- //this->laserMsg.header.stamp.sec =
- //(unsigned long)floor(world->SimTimeNow() / 1e6);
- //this->laserMsg.header.stamp.nsec =
- //(unsigned long)rint(1e3 * (world->SimTimeNow() -
- //this->laserMsg.header.stamp.sec * 1e6));
- publish("scan",this->laserMsg);
+ this->laserMsg.header.stamp = sim_time;
+ publish("base_scan",this->laserMsg);
}
+
+ // 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->lasermodel->GetPose();
+ tf.sendEuler("base_laser",
+ "base",
+ lp.x,
+ lp.y,
+ 0.0,
+ lp.a,
+ 0.0,
+ 0.0,
+ sim_time);
// Get latest odometry data
// Translate into ROS message format and publish
@@ -265,18 +276,9 @@
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 = "odom";
-
- this->odomMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
- //this->odomMsg.header.stamp.sec =
- //(unsigned long)floor(world->SimTimeNow() / 1e6);
- //this->odomMsg.header.stamp.nsec =
- //(unsigned long)rint(1e3 * (world->SimTimeNow() -
- //this->odomMsg.header.stamp.sec * 1e6));
- // printf("%u \n",world->SimTimeNow());
- //printf("time: %u, %u \n",odomMsg.header.stamp.sec, odomMsg.header.stamp.nsec);
-
+ this->odomMsg.header.stamp = sim_time;
+ publish("odom",this->odomMsg);
tf.sendInverseEuler("odom",
"base",
odomMsg.pos.x,
@@ -285,10 +287,8 @@
odomMsg.pos.th,
0.0,
0.0,
- odomMsg.header.stamp);
+ sim_time);
- publish("odom",this->odomMsg);
-
// Also publish the ground truth pose
Stg::stg_pose_t gpose = this->positionmodel->GetGlobalPose();
// Use libTF to construct our outgoing message
@@ -308,7 +308,7 @@
this->groundTruthMsg.transform.rotation.w = tmpPose3D.orientation.w;
this->groundTruthMsg.header.frame_id = "odom";
- this->groundTruthMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
+ this->groundTruthMsg.header.stamp = sim_time;
publish("base_pose_ground_truth", this->groundTruthMsg);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
