Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7707] pkg/trunk
|
From: <ge...@us...> - 2008-12-06 00:11:55
|
Revision: 7707
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7707&view=rev
Author: gerkey
Date: 2008-12-06 00:11:46 +0000 (Sat, 06 Dec 2008)
Log Message:
-----------
added watchdog timeout to rosstage
Modified Paths:
--------------
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_gazebo.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_gazebo_wg_amcl.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.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/simulators/rosstage/rosstage.cc
Added Paths:
-----------
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml
Removed Paths:
-------------
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/stage.xml
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_gazebo.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_gazebo.xml 2008-12-05 23:48:46 UTC (rev 7706)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_gazebo.xml 2008-12-06 00:11:46 UTC (rev 7707)
@@ -18,7 +18,7 @@
<node pkg="nav_view" type="nav_view" args="odom:=localizedpose" respawn="false"/>
<!-- Now launch controller node required -->
- <include file="$(find highlevel_controllers)/test/launch_move_base.xml"/>
+ <include file="$(find test_highlevel_controllers)/test/launch_move_base.xml"/>
</group>
</launch>
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_gazebo_wg_amcl.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_gazebo_wg_amcl.xml 2008-12-05 23:48:46 UTC (rev 7706)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_gazebo_wg_amcl.xml 2008-12-06 00:11:46 UTC (rev 7707)
@@ -32,7 +32,7 @@
<node pkg="nav_view" type="nav_view" args="odom:=localizedpose" respawn="false"/>
<!-- Now launch controller node required -->
- <include file="$(find highlevel_controllers)/test/launch_move_base.xml"/>
+ <include file="$(find test_highlevel_controllers)/test/launch_move_base.xml"/>
</group>
</launch>
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2008-12-05 23:48:46 UTC (rev 7706)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2008-12-06 00:11:46 UTC (rev 7707)
@@ -19,6 +19,6 @@
<param name="/costmap_2d/circumscribed_radius" value="0.46"/>
<param name="/costmap_2d/inscribed_radius" value="0.325"/>
<param name="/costmap_2d/weight" value="0.1"/>
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="base_scan:=base_scan tilt_scan:=tilt_scan" respawn="true" />
+ <node pkg="highlevel_controllers" type="move_base_sbpl" respawn="false" />
</group>
</launch>
Deleted: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml 2008-12-05 23:48:46 UTC (rev 7706)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml 2008-12-06 00:11:46 UTC (rev 7707)
@@ -1,8 +0,0 @@
-
-<launch>
- <master auto="start"/>
- <group name="wg">
- <include file="$(find highlevel_controllers)/test/stage.xml"/>
- </group>
-</launch>
-
Copied: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml (from rev 7668, pkg/trunk/highlevel/test_highlevel_controllers/test/stage.xml)
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml (rev 0)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml 2008-12-06 00:11:46 UTC (rev 7707)
@@ -0,0 +1,28 @@
+<launch>
+ <master auto="start"/>
+ <group name="wg">
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="$(find 2dnav_stage)/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)/willow-full.pgm 0.1" respawn="false" />
+
+ <!-- TODO: remove this remap after fake_localization is fixed to listen
+ to base_pose_ground_truth, and gazebo is fixed to publish on that topic -->
+ <remap from="base_pose_gazebo_ground_truth" to="base_pose_ground_truth"/>
+ <param name="max_publish_frequency" value="20.0"/>
+ <node pkg="fake_localization" type="fake_localization" respawn="false" />
+ <node pkg="nav_view" type="nav_view" respawn="false"/>
+
+ <!-- Now launch controller node required -->
+ <include file="$(find test_highlevel_controllers)/test/launch_move_base.xml"/>
+
+ <!-- Set parameters for mailing -->
+ <param name="recharge/email_addresses" value="mc...@wi..."/>
+ <param name="recharge/subject_plugin" value="Robot Needs To Be Plugged In"/>
+ <param name="recharge/subject_unplug" value="Robot Needs To Be Unplugged"/>
+ <param name="recharge/body_plugin" value="Hello, could you please plug me in?\nThanks, PR2"/>
+ <param name="recharge/body_unplug" value="Hello, could you please unplug me?\nThanks, PR2"/>
+ <param name="recharge/mail_client" value="mailx -s"/>
+ <node pkg="highlevel_controllers" type="recharge_controller" args="battery_state:=bogus_battery_state" respawn="true" />
+ </group>
+</launch>
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 2008-12-05 23:48:46 UTC (rev 7706)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_2.5cm.xml 2008-12-06 00:11:46 UTC (rev 7707)
@@ -1,7 +1,9 @@
<launch>
<master auto="start"/>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="-g $(find 2dnav_stage)/willow-pr2-2.5cm.world" respawn="false" />
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="-g $(find 2dnav_stage)/willow-pr2-2.5cm.world" respawn="false" >
+ <param name="base_watchdog_timeout" value="0.2"/>
+ </node>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/willow-full-0.025.pgm 0.025" respawn="false" />
<!-- TODO: remove this remap after fake_localization is fixed to listen
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_5cm.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_5cm.xml 2008-12-05 23:48:46 UTC (rev 7706)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_5cm.xml 2008-12-06 00:11:46 UTC (rev 7707)
@@ -1,7 +1,9 @@
<launch>
<master auto="start"/>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/willow-pr2-5cm.world" respawn="false" />
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="$(find 2dnav_stage)/willow-pr2-5cm.world" respawn="false" >
+ <param name="base_watchdog_timeout" value="0.2"/>
+ </node>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/willow-full-0.05.pgm 0.05" respawn="false" />
<!-- TODO: remove this remap after fake_localization is fixed to listen
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_amcl_5cm.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_amcl_5cm.xml 2008-12-05 23:48:46 UTC (rev 7706)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_amcl_5cm.xml 2008-12-06 00:11:46 UTC (rev 7707)
@@ -1,7 +1,9 @@
<launch>
<master auto="start"/>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/willow-pr2-5cm.world" respawn="false" />
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="$(find 2dnav_stage)/willow-pr2-5cm.world" respawn="false" >
+ <param name="base_watchdog_timeout" value="0.2"/>
+ </node>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/willow-full-0.05.pgm 0.05" respawn="false" />
<param name="pf_laser_max_beams" value="20"/>
<param name="pf_min_samples" value="500"/>
Deleted: pkg/trunk/highlevel/test_highlevel_controllers/test/stage.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/stage.xml 2008-12-05 23:48:46 UTC (rev 7706)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/stage.xml 2008-12-06 00:11:46 UTC (rev 7707)
@@ -1,21 +0,0 @@
-<launch>
- <!-- Sends robot description data to the param server -->
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
-
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-erratic.world" respawn="false" />
-
- <node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full.pgm 0.1" respawn="false" />
-
- <!--node pkg="amcl_player" type="amcl_player" respawn="false" /-->
- <remap from="base_pose_gazebo_ground_truth" to="base_pose_ground_truth"/>
- <param name="max_publish_frequency" value="20.0"/>
- <node pkg="fake_localization" type="fake_localization" respawn="false" />
-
- <param name="move_base/frequency" value="20.0"/>
- <param name="move_base/plannerTimeLimit" value="1.0"/>
- <node pkg="highlevel_controllers" type="move_base_sbpl" respawn="false" />
-
- <node pkg="highlevel_controllers" type="recharge_controller" respawn="false" />
-
- <node pkg="nav_view" type="nav_view" respawn="true"/>
-</launch>
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-12-05 23:48:46 UTC (rev 7706)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-12-06 00:11:46 UTC (rev 7707)
@@ -116,6 +116,13 @@
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.
@@ -161,6 +168,7 @@
this->lock.lock();
this->positionmodel->SetSpeed(this->velMsg.vx, this->velMsg.vy, this->velMsg.vw);
+ this->base_last_cmd = this->sim_time;
this->lock.unlock();
}
@@ -171,6 +179,12 @@
this->lasermodel = NULL;
this->positionmodel = NULL;
+ 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 );
@@ -229,9 +243,12 @@
// Let the simulator update (it will sleep if there's time)
this->world->Update();
- ros::Time sim_time;
- sim_time.fromSec(world->SimTimeNow() / 1e6);
+ this->sim_time.fromSec(world->SimTimeNow() / 1e6);
+ if((this->base_watchdog_timeout.toSec() > 0.0) &&
+ ((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
+ this->positionmodel->SetSpeed(0.0, 0.0, 0.0);
+
// Get latest laser data
Stg::stg_laser_sample_t* samples = this->lasermodel->GetSamples();
if(samples)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
