Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22930] pkg/trunk
|
From: <ei...@us...> - 2009-08-26 01:38:43
|
Revision: 22930
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22930&view=rev
Author: eitanme
Date: 2009-08-26 01:38:16 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Removing the old move_base executable, updating all launch files to include a move_base_translator so that things work with the old action interface. Also, updating tests so that they'll pass using the _old suffix to use the old action_client interface. Tried to get everything, but its possible I didn't... launch files are hard to test... cross your fingers
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base/move_base.xml
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base_local/move_base_local.xml
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/rviz_move_base.launch
pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch
pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml
pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch
pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp
pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml
pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch
pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp
pkg/trunk/nav/visual_nav/launch/move_base.launch
pkg/trunk/nav/visual_nav/launch/move_base_robot.launch
pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h
pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp
pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml
pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server_imp.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h
pkg/trunk/stacks/navigation/move_base/CMakeLists.txt
pkg/trunk/stacks/navigation/move_base/manifest.xml
pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
pkg/trunk/stacks/navigation/stack.xml
pkg/trunk/stacks/trex/trex_pr2_writing_test/src/test_move_to_helper.cpp
Removed Paths:
-------------
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base_old.h
pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base/move_base.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base/move_base.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base/move_base.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -2,7 +2,13 @@
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
<node pkg="mux" type="throttle" args="3.0 /move_base/local_costmap/voxel_grid /move_base/local_costmap/voxel_grid_throttled" />
-<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen" machine="four">
+
+<!-- Run the move base translator -->
+<node pkg="move_base_client" type="move_base_translator" name="move_base_translator" machine="four">
+ <param name="action_name" value="move_base" />
+</node>
+
+<node pkg="move_base" type="move_base" name="move_base" output="screen" machine="four">
<remap from="odom" to="pr2_odometry/odom" />
<param name="controller_frequency" value="10.0" />
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base_local/move_base_local.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base_local/move_base_local.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base_local/move_base_local.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -2,6 +2,12 @@
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
<node pkg="mux" type="throttle" args="3.0 /move_base_local/local_costmap/voxel_grid /move_base_local/local_costmap/voxel_grid_throttled" />
+
+<!-- Run the move base translator -->
+<node pkg="move_base_client" type="move_base_translator" name="move_base_local_translator" machine="four">
+ <param name="action_name" value="move_base_local" />
+</node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base_local" output="screen" machine="three">
<remap from="odom" to="base/odom" />
<param name="controller_frequency" value="10.0" />
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/rviz_move_base.launch
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/rviz_move_base.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/rviz_move_base.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,6 +1,6 @@
<launch>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find 2dnav_pr2)/rviz/move_base.vcg">
- <remap from="goal" to="/move_base/activate" />
+ <remap from="goal" to="/move_base_old/activate" />
</node>
<node pkg="costmap_2d" type="costmap_2d_markers" name="voxel_visualizer">
<remap from="voxel_grid" to="/move_base/local_costmap/voxel_grid_throttled"/>
Modified: pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch
===================================================================
--- pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,5 +1,11 @@
<launch>
<master auto="start"/>
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="footprint_padding" value="0.01" />
Modified: pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml
===================================================================
--- pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -26,6 +26,12 @@
<param name="laser_max_range" value="30.0" />
</node>
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
+
<!-- for moving -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="10.0" />
Modified: pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -2,6 +2,12 @@
<master auto="start"/>
<group name="wg">
<param name="/use_sim_time" value="true"/>
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" >
<param name="global_frame" value="map" />
Modified: pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -10,7 +10,6 @@
<depend package="map_server"/>
<depend package="fake_localization"/>
<depend package="teleop_arm_keyboard"/>
- <depend package="trex_pr2"/>
<depend package="robot_pose_ekf"/>
<depend package="pr2_gazebo"/>
<depend package="tf"/>
Modified: pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-26 01:38:16 UTC (rev 22930)
@@ -183,7 +183,7 @@
self.bumped = True
def stateInput(self, state):
- if self.publish_goal:
+ if self.publish_goal and state.status.value == state.status.ACTIVE:
state_eul = tft.euler_from_quaternion([state.goal.pose.orientation.x,state.goal.pose.orientation.y,state.goal.pose.orientation.z,state.goal.pose.orientation.w])
print "target: ", self.target_x, ",", self.target_y, ",", self.target_t
print "state.goal: (", state.goal.pose.position.x, ",", state.goal.pose.position.y, ",", state.goal.pose.position.z \
@@ -219,13 +219,13 @@
print "LNK\n"
#pub_base = rospy.Publisher("cmd_vel", BaseVel)
- pub_goal = rospy.Publisher("/move_base/activate", PoseStamped)
+ pub_goal = rospy.Publisher("/move_base_old/activate", PoseStamped)
pub_pose = rospy.Publisher("initialpose" , PoseWithCovarianceStamped)
rospy.Subscriber("base_pose_ground_truth", Odometry , self.p3dInput)
rospy.Subscriber("pr2_odometry/odom" , Odometry , self.odomInput)
rospy.Subscriber("base_bumper/info" , String , self.bumpedInput)
rospy.Subscriber("torso_lift_bumper/info", String , self.bumpedInput)
- rospy.Subscriber("/move_base/feedback" , MoveBaseState , self.stateInput)
+ rospy.Subscriber("/move_base_old/feedback" , MoveBaseState , self.stateInput)
rospy.Subscriber("/amcl_pose" , PoseWithCovarianceStamped , self.amclInput)
# below only for debugging build 303, base not moving
@@ -421,3 +421,4 @@
rostest.run(PKG, sys.argv[0], NavStackTest, sys.argv) #, text_mode=True)
+
Modified: pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -121,7 +121,7 @@
robot_actions::ActionClient<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door> push_door("push_door");
robot_actions::ActionClient<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> release_handle("release_handle");
robot_actions::ActionClient<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door> move_base_door("move_base_door");
- robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local");
+ robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local_old");
door_msgs::Door tmp_door;
door_msgs::Door backup_door;
Modified: pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -107,7 +107,7 @@
robot_actions::ActionClient<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> release_handle("release_handle");
robot_actions::ActionClient<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door> move_base_door("move_base_door");
robot_actions::ActionClient<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door> sbpl_door_planner("sbpl_door_planner");
- robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local");
+ robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local_old");
robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_arm");
door_msgs::Door tmp_door;
Modified: pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,5 +1,11 @@
<launch>
- <node pkg="move_base" type="move_base_new" respawn="false" name="move_base" output="screen">
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="footprint_padding" value="0.01" />
<param name="controller_frequency" value="10.0" />
Modified: pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -12,6 +12,12 @@
<!-- BEGIN ROBOT 0 -->
<group ns="robot_0">
<param name="~tf_prefix" value="robot_0" />
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="10.0" />
<rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
@@ -39,6 +45,12 @@
<!-- BEGIN ROBOT 1 -->
<group ns="robot_1">
<param name="~tf_prefix" value="robot_1" />
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="10.0" />
<rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
Modified: pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -84,7 +84,7 @@
robot_actions::ActionClient<pr2_robot_actions::SwitchControllers, pr2_robot_actions::SwitchControllersState, std_msgs::Empty>
switch_controllers("switch_controllers");
robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped>
- move_base_local("move_base_local");
+ move_base_local("move_base_local_old");
Duration(1.0).sleep();
Modified: pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -115,7 +115,7 @@
robot_actions::ActionClient<geometry_msgs::PointStamped, pr2_robot_actions::DetectOutletState, geometry_msgs::PoseStamped>
detect_outlet_coarse("detect_outlet_coarse");
robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped>
- move_base_local("move_base_local");
+ move_base_local("move_base_local_old");
Duration(1.0).sleep();
Modified: pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -122,7 +122,7 @@
robot_actions::ActionClient<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> plug_in("plug_in");
robot_actions::ActionClient<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> unplug("unplug");
robot_actions::ActionClient< plugs_msgs::PlugStow, pr2_robot_actions::StowPlugState, std_msgs::Empty> stow_plug("stow_plug");
- robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local");
+ robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local_old");
timeout_medium.sleep();
Modified: pkg/trunk/nav/visual_nav/launch/move_base.launch
===================================================================
--- pkg/trunk/nav/visual_nav/launch/move_base.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/nav/visual_nav/launch/move_base.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,6 +1,11 @@
<launch>
<master auto="start"/>
<group>
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="footprint_padding" value="0.01" />
Modified: pkg/trunk/nav/visual_nav/launch/move_base_robot.launch
===================================================================
--- pkg/trunk/nav/visual_nav/launch/move_base_robot.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/nav/visual_nav/launch/move_base_robot.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -8,6 +8,12 @@
<include file="$(find 2dnav_pr2)/config/ground_plane.xml" />
<node pkg="mux" type="throttle" args="3.0 /move_base/base_local_planner/costmap/voxel_grid /move_base/base_local_planner/costmap/voxel_grid_throttled" />
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator" machine="four">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" machine="three">
<param name="footprint_padding" value="0.01" />
Modified: pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h
===================================================================
--- pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -102,6 +102,9 @@
// Convert from the old goal type to the new goal type
Goal new_goal = from_old_goal_func_(old_goal);
+ //wait for the action server to start before passing the goal on
+ ac_.waitForActionServerToStart(ros::Duration(5.0));
+
// Send the goal to the action server
ac_.sendGoal(new_goal,
boost::bind(&ActionTranslatorT::handleDone, this, _1, _2),
Modified: pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -67,9 +67,11 @@
ros::init(argc, argv, "move_base_action_translator");
ros::NodeHandle n;
+ std::string action_name;
+ n.param("~action_name", action_name, std::string("move_base"));
action_translator::ActionTranslator<move_base_msgs::MoveBaseAction, PoseStamped, PoseStamped>
- translator("move_base", &fromOldGoal, NULL, NULL);
+ translator(action_name, &fromOldGoal, NULL, NULL);
robot_actions::ActionRunner runner(10.0);
runner.connect<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped>(translator);
Modified: pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -13,7 +13,13 @@
<!-- The naviagtion stack and associated parameters -->
<!--include file="$(find 2dnav_pr2)/move_base_local/move_base_local.xml" /-->
<node pkg="mux" type="throttle" args="3.0 /move_base_local/local_costmap/voxel_grid /move_base_local/local_costmap/voxel_grid_throttled" />
- <node pkg="move_base" type="move_base_new" respawn="false" name="move_base_local" output="screen" machine="three">
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_local_translator" machine="four">
+ <param name="action_name" value="move_base_local" />
+ </node>
+
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base_local" output="screen" machine="three">
<param name="controller_frequency" value="10.0" />
<param name="base_global_planner" value="CarrotPlanner" />
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -80,12 +80,23 @@
* @param name The name of the action
* @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire
* @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire
+ * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up
*/
ActionServer(ros::NodeHandle n, std::string name,
boost::function<void (GoalHandle)> goal_cb = boost::function<void (GoalHandle)>(),
- boost::function<void (GoalHandle)> cancel_cb = boost::function<void (GoalHandle)>());
+ boost::function<void (GoalHandle)> cancel_cb = boost::function<void (GoalHandle)>(),
+ bool auto_start = true);
/**
+ * @brief Constructor for an ActionServer
+ * @param n A NodeHandle to create a namespace under
+ * @param name The name of the action
+ * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up
+ */
+ ActionServer(ros::NodeHandle n, std::string name,
+ bool auto_start = true);
+
+ /**
* @brief Register a callback to be invoked when a new goal is received, this will replace any previously registered callback
* @param cb The callback to invoke
*/
@@ -97,8 +108,18 @@
*/
void registerCancelCallback(boost::function<void (GoalHandle)> cb);
+ /**
+ * @brief Explicitly start the action server, used it auto_start is set to false
+ */
+ void start();
+
private:
/**
+ * @brief Initialize all ROS connections and setup timers
+ */
+ void initialize();
+
+ /**
* @brief Publishes a result for a given goal
* @param status The status of the goal with which the result is associated
* @param result The result to publish
@@ -154,6 +175,7 @@
friend class HandleTrackerDeleter<ActionSpec>;
GoalIDGenerator id_generator_;
+ bool started_;
};
};
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server_imp.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server_imp.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server_imp.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -39,9 +39,23 @@
namespace actionlib {
template <class ActionSpec>
ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
+ bool auto_start)
+ : ActionServer<ActionSpec>::ActionServer(n, name, boost::function<void (GoalHandle)>(), boost::function<void (GoalHandle)>(), auto_start) {}
+
+ template <class ActionSpec>
+ ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
boost::function<void (GoalHandle)> goal_cb,
- boost::function<void (GoalHandle)> cancel_cb)
- : node_(n, name), goal_callback_(goal_cb), cancel_callback_(cancel_cb) {
+ boost::function<void (GoalHandle)> cancel_cb,
+ bool auto_start)
+ : node_(n, name), goal_callback_(goal_cb), cancel_callback_(cancel_cb), started_(auto_start) {
+
+ //if we're to autostart... then we'll initialize things
+ if(started_)
+ initialize();
+ }
+
+ template <class ActionSpec>
+ void ActionServer<ActionSpec>::initialize(){
status_pub_ = node_.advertise<actionlib_msgs::GoalStatusArray>("status", 1);
result_pub_ = node_.advertise<ActionResult>("result", 1);
feedback_pub_ = node_.advertise<ActionFeedback>("feedback", 1);
@@ -61,9 +75,8 @@
status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency),
boost::bind(&ActionServer::publishStatus, this, _1));
+ }
- }
-
template <class ActionSpec>
void ActionServer<ActionSpec>::registerGoalCallback(boost::function<void (GoalHandle)> cb){
goal_callback_ = cb;
@@ -97,6 +110,11 @@
template <class ActionSpec>
void ActionServer<ActionSpec>::cancelCallback(const boost::shared_ptr<const actionlib_msgs::GoalID>& goal_id){
boost::recursive_mutex::scoped_lock lock(lock_);
+
+ //if we're not started... then we're not actually going to do anything
+ if(!started_)
+ return;
+
//we need to handle a cancel for the user
ROS_DEBUG("The action server has received a new cancel request");
bool goal_id_found = false;
@@ -153,6 +171,10 @@
void ActionServer<ActionSpec>::goalCallback(const boost::shared_ptr<const ActionGoal>& goal){
boost::recursive_mutex::scoped_lock lock(lock_);
+ //if we're not started... then we're not actually going to do anything
+ if(!started_)
+ return;
+
ROS_DEBUG("The action server has received a new goal request");
//we need to check if this goal already lives in the status list
@@ -191,8 +213,19 @@
}
template <class ActionSpec>
+ void ActionServer<ActionSpec>::start(){
+ initialize();
+ started_ = true;
+ }
+
+ template <class ActionSpec>
void ActionServer<ActionSpec>::publishStatus(const ros::TimerEvent& e){
boost::recursive_mutex::scoped_lock lock(lock_);
+
+ //we won't publish status unless we've been started
+ if(!started_)
+ return;
+
publishStatus();
}
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -69,8 +69,9 @@
* @param execute_cb Optional callback that gets called in a separate thread whenever
* a new goal is received, allowing users to have blocking callbacks.
* Adding an execute callback also deactivates the goalCallback.
+ * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up
*/
- SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = NULL);
+ SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = NULL, bool auto_start = true);
~SimpleActionServer();
@@ -149,6 +150,11 @@
*/
void registerPreemptCallback(boost::function<void ()> cb);
+ /**
+ * @brief Explicitly start the action server, used it auto_start is set to false
+ */
+ void start();
+
private:
/**
* @brief Callback for when the ActionServer receives a new goal and passes it on
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -37,13 +37,14 @@
namespace actionlib {
template <class ActionSpec>
- SimpleActionServer<ActionSpec>::SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback)
+ SimpleActionServer<ActionSpec>::SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback, bool auto_start)
: n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback), need_to_terminate_(false) {
//create the action server
as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
boost::bind(&SimpleActionServer::goalCallback, this, _1),
- boost::bind(&SimpleActionServer::preemptCallback, this, _1)));
+ boost::bind(&SimpleActionServer::preemptCallback, this, _1),
+ auto_start));
if (execute_callback_ != NULL)
{
@@ -263,5 +264,10 @@
}
}
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::start(){
+ as_->start();
+ }
+
};
Modified: pkg/trunk/stacks/navigation/move_base/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/navigation/move_base/CMakeLists.txt 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/navigation/move_base/CMakeLists.txt 2009-08-26 01:38:16 UTC (rev 22930)
@@ -7,13 +7,6 @@
rosbuild_add_boost_directories()
-# Library
-rosbuild_add_library(move_base src/move_base_old.cpp)
-rosbuild_link_boost(move_base system)
-
# move_base
-rosbuild_add_executable(bin/move_base src/move_base_old.cpp)
+rosbuild_add_executable(bin/move_base src/move_base.cpp)
rosbuild_link_boost(bin/move_base system)
-
-rosbuild_add_executable(bin/move_base_new src/move_base.cpp)
-rosbuild_link_boost(bin/move_base_new system)
Deleted: pkg/trunk/stacks/navigation/move_base/include/move_base/move_base_old.h
===================================================================
--- pkg/trunk/stacks/navigation/move_base/include/move_base/move_base_old.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/navigation/move_base/include/move_base/move_base_old.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,173 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef NAV_MOVE_BASE_OLD_ACTION_H_
-#define NAV_MOVE_BASE_OLD_ACTION_H_
-#include <robot_actions/action.h>
-#include <robot_actions/action_runner.h>
-#include <nav_robot_actions/MoveBaseState.h>
-#include <nav_core/base_local_planner.h>
-#include <nav_core/base_global_planner.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <ros/ros.h>
-#include <costmap_2d/costmap_2d_ros.h>
-#include <costmap_2d/costmap_2d.h>
-#include <vector>
-#include <string>
-#include <nav_msgs/GetPlan.h>
-#include <visualization_msgs/Marker.h>
-#include <geometry_msgs/Twist.h>
-
-#include <pluginlib/class_loader.h>
-
-namespace move_base {
- enum MoveBaseState {
- PLANNING,
- CONTROLLING,
- CLEARING
- };
-
- enum ClearingState {
- CONSERVATIVE_RESET,
- IN_PLACE_ROTATION,
- AGGRESSIVE_RESET,
- ABORT
- };
-
- /**
- * @class MoveBase
- * @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
- */
- class MoveBase : public robot_actions::Action<geometry_msgs::PoseStamped, geometry_msgs::PoseStamped> {
- public:
- /**
- * @brief Constructor for the actions
- * @param name The name of the action
- * @param tf A reference to a TransformListener
- */
- MoveBase(std::string name, tf::TransformListener& tf);
-
- /**
- * @brief Destructor - Cleans up
- */
- virtual ~MoveBase();
-
- /**
- * @brief Runs whenever a new goal is sent to the move_base
- * @param goal The goal to pursue
- * @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot
- * @return The result of the execution, ie: Success, Preempted, Aborted, etc.
- */
- virtual robot_actions::ResultStatus execute(const geometry_msgs::PoseStamped& goal, geometry_msgs::PoseStamped& feedback);
-
- private:
- /**
- * @brief A service call that can be made when the action is inactive that will return a plan
- * @param req The goal request
- * @param resp The plan request
- * @return True if planning succeeded, false otherwise
- */
- bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
-
- /**
- * @brief Make a new global plan
- * @param goal The goal to plan to
- * @param plan Will be filled in with the plan made by the planner
- * @return True if planning succeeds, false otherwise
- */
- bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
-
- /**
- * @brief Publish a goal to the visualizer
- * @param goal The goal to visualize
- */
- void publishGoal(const geometry_msgs::PoseStamped& goal);
-
- /**
- * @brief Resets the costmaps to the static map outside a given window
- * @param size_x The x size of the window
- * @param size_y The y size of the window
- */
- void resetCostmaps(double size_x, double size_y);
-
- /**
- * @brief Clears obstacles within a window around the robot
- * @param size_x The x size of the window
- * @param size_y The y size of the window
- */
- void clearCostmapWindows(double size_x, double size_y);
-
- /**
- * @brief Publishes a velocity command of zero to the base
- */
- void publishZeroVelocity();
-
- /**
- * @brief Perform an in-place rotation of the base to attempt to clear out space
- */
- void rotateRobot();
-
- /**
- * @brief Reset the state of the move_base action and send a zero velocity command to the base
- */
- void resetState();
-
- ros::NodeHandle ros_node_;
- tf::TransformListener& tf_;
- nav_core::BaseLocalPlanner* tc_;
- costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
-
- nav_core::BaseGlobalPlanner* planner_;
- std::string robot_base_frame_, global_frame_;
-
- tf::Stamped<tf::Pose> global_pose_;
- double controller_frequency_, inscribed_radius_, circumscribed_radius_;
- double planner_patience_, controller_patience_;
- double conservative_reset_dist_, clearing_radius_;
- ros::Publisher vis_pub_, vel_pub_;
- ros::ServiceServer make_plan_srv_;
- bool shutdown_costmaps_;
-
- MoveBaseState state_;
- ClearingState clearing_state_;
-
- pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
- pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;
-
- };
-};
-#endif
-
Modified: pkg/trunk/stacks/navigation/move_base/manifest.xml
===================================================================
--- pkg/trunk/stacks/navigation/move_base/manifest.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/navigation/move_base/manifest.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -15,10 +15,8 @@
<depend package="move_base_msgs"/>
<depend package="geometry_msgs"/>
<depend package="visualization_msgs"/>
- <depend package="robot_actions"/>
<depend package="actionlib"/>
<depend package="nav_core"/>
- <depend package="nav_robot_actions"/>
<depend package="pluginlib"/>
<depend package="costmap_2d"/>
<depend package="tf"/>
@@ -30,6 +28,6 @@
<depend package="navfn"/>
<export>
- <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lmove_base"/>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread`"/>
</export>
</package>
Modified: pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -42,7 +42,7 @@
MoveBase::MoveBase(std::string name, tf::TransformListener& tf) :
tf_(tf),
- as_(ros::NodeHandle(), name, boost::bind(&MoveBase::executeCb, this, _1)),
+ as_(ros::NodeHandle(), name, boost::bind(&MoveBase::executeCb, this, _1), false),
tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
planner_(NULL), bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"){
@@ -126,6 +126,8 @@
//the initial clearing state will be to conservatively clear the costmaps
clearing_state_ = CONSERVATIVE_RESET;
+ //we're all set up now so we can start the action server
+ as_.start();
}
void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
Deleted: pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,561 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#include <move_base/move_base_old.h>
-#include <cstdlib>
-#include <ctime>
-
-using namespace costmap_2d;
-using namespace robot_actions;
-
-namespace move_base {
-
- MoveBase::MoveBase(std::string name, tf::TransformListener& tf) :
- Action<geometry_msgs::PoseStamped, geometry_msgs::PoseStamped>(name), tf_(tf),
- tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
- planner_(NULL), bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
- blp_loader_("nav_core", "nav_core::BaseLocalPlanner") {
-
- //get some parameters that will be global to the move base node
- std::string global_planner, local_planner;
- ros_node_.param("~base_global_planner", global_planner, std::string("NavfnROS"));
- ros_node_.param("~base_local_planner", local_planner, std::string("TrajectoryPlannerROS"));
- ros_node_.param("~global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
- ros_node_.param("~global_costmap/global_frame", global_frame_, std::string("/map"));
- ros_node_.param("~controller_frequency", controller_frequency_, 20.0);
- ros_node_.param("~planner_patience", planner_patience_, 5.0);
- ros_node_.param("~controller_patience", controller_patience_, 15.0);
-
- //for comanding the base
- vel_pub_ = ros_node_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
- vis_pub_ = ros_node_.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
-
- //we'll assume the radius of the robot to be consistent with what's specified for the costmaps
- ros_node_.param("~local_costmap/inscribed_radius", inscribed_radius_, 0.325);
- ros_node_.param("~local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
- ros_node_.param("~clearing_radius", clearing_radius_, circumscribed_radius_);
- ros_node_.param("~conservative_reset_dist", conservative_reset_dist_, 3.0);
-
- ros_node_.param("~shutdown_costmaps", shutdown_costmaps_, false);
-
- //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
- planner_costmap_ros_ = new Costmap2DROS("global_costmap", tf_);
-
- //initialize the global planner
- try {
- planner_ = bgp_loader_.createClassInstance(global_planner);
- planner_->initialize(global_planner, planner_costmap_ros_);
- } catch (const std::runtime_error& ex)
- {
- ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
- exit(0);
- }
-
- ROS_INFO("MAP SIZE: %d, %d", planner_costmap_ros_->getSizeInCellsX(), planner_costmap_ros_->getSizeInCellsY());
-
- //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
- controller_costmap_ros_ = new Costmap2DROS("local_costmap", tf_);
-
- //create a local planner
- try {
- tc_ = blp_loader_.createClassInstance(local_planner);
- tc_->initialize(local_planner, &tf_, controller_costmap_ros_);
- } catch (const std::runtime_error& ex)
- {
- ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
- exit(0);
- }
-
- //advertise a service for getting a plan
- make_plan_srv_ = ros_node_.advertiseService("~make_plan", &MoveBase::planService, this);
-
- //initially clear any unknown space around the robot
- planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
- controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
-
- //if we shutdown our costmaps when we're deactivated... we'll do that now
- if(shutdown_costmaps_){
- planner_costmap_ros_->stop();
- controller_costmap_ros_->stop();
- }
-
- //initially, we'll need to make a plan
- state_ = PLANNING;
-
- //the initial clearing state will be to conservatively clear the costmaps
- clearing_state_ = CONSERVATIVE_RESET;
-
- }
-
- void MoveBase::clearCostmapWindows(double size_x, double size_y){
- tf::Stamped<tf::Pose> global_pose;
-
- //clear the planner's costmap
- planner_costmap_ros_->getRobotPose(global_pose);
-
- std::vector<geometry_msgs::Point> clear_poly;
- double x = global_pose.getOrigin().x();
- double y = global_pose.getOrigin().y();
- geometry_msgs::Point pt;
-
- pt.x = x - size_x / 2;
- pt.y = y - size_x / 2;
- clear_poly.push_back(pt);
-
- pt.x = x + size_x / 2;
- pt.y = y - size_x / 2;
- clear_poly.push_back(pt);
-
- pt.x = x + size_x / 2;
- pt.y = y + size_x / 2;
- clear_poly.push_back(pt);
-
- pt.x = x - size_x / 2;
- pt.y = y + size_x / 2;
- clear_poly.push_back(pt);
-
- planner_costmap_ros_->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
-
- //clear the controller's costmap
- controller_costmap_ros_->getRobotPose(global_pose);
-
- clear_poly.clear();
- x = global_pose.getOrigin().x();
- y = global_pose.getOrigin().y();
-
- pt.x = x - size_x / 2;
- pt.y = y - size_x / 2;
- clear_poly.push_back(pt);
-
- pt.x = x + size_x / 2;
- pt.y = y - size_x / 2;
- clear_poly.push_back(pt);
-
- pt.x = x + size_x / 2;
- pt.y = y + size_x / 2;
- clear_poly.push_back(pt);
-
- pt.x = x - size_x / 2;
- pt.y = y + size_x / 2;
- clear_poly.push_back(pt);
-
- controller_costmap_ros_->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
- }
-
- bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
- if(isActive()){
- ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
- return false;
- }
-
- //make sure we have a costmap for our planner
- if(planner_costmap_ros_ == NULL){
- ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap");
- return false;
- }
-
- tf::Stamped<tf::Pose> global_pose;
- if(!planner_costmap_ros_->getRobotPose(global_pose)){
- ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
- return false;
- }
-
- geometry_msgs::PoseStamped start;
- tf::poseStampedTFToMsg(global_pose, start);
-
- //update the copy of the costmap the planner uses
- clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
-
- //if we have a tolerance on the goal point that is greater
- //than the resolution of the map... compute the full potential function
- double resolution = planner_costmap_ros_->getResolution();
- std::vector<geometry_msgs::PoseStamped> global_plan;
- geometry_msgs::PoseStamped p;
- p = req.goal;
- p.pose.position.y = req.goal.pose.position.y - req.tolerance;
- bool found_legal = false;
- while(!found_legal && p.pose.position.y <= req.goal.pose.position.y + req.tolerance){
- p.pose.position.x = req.goal.pose.position.x - req.tolerance;
- while(!found_legal && p.pose.position.x <= req.goal.pose.position.x + req.tolerance){
- if(planner_->makePlan(start, p, global_plan)){
- if(!global_plan.empty()){
- global_plan.push_back(p);
- found_legal = true;
- }
- else
- ROS_DEBUG("Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
- }
- p.pose.position.x += resolution*3.0;
- }
- p.pose.position.y += resolution*3.0;
- }
-
- //copy the plan into a message to send out
- resp.plan.set_poses_size(global_plan.size());
- for(unsigned int i = 0; i < global_plan.size(); ++i){
- resp.plan.poses[i] = global_plan[i];
- }
-
-
-
- return true;
- }
-
- MoveBase::~MoveBase(){
- if(planner_ != NULL)
- delete planner_;
-
- if(tc_ != NULL)
- delete tc_;
-
- if(planner_costmap_ros_ != NULL)
- delete planner_costmap_ros_;
-
- if(controller_costmap_ros_ != NULL)
- delete controller_costmap_ros_;
- }
-
- void MoveBase::publishGoal(const geometry_msgs::PoseStamped& goal){
- visualization_msgs::Marker marker;
- marker.header = goal.header;
- marker.ns = "move_base";
- marker.id = 0;
- marker.type = visualization_msgs::Marker::ARROW;
- marker.pose = goal.pose;
- marker.scale.x = 0.5;
- marker.scale.y = 0.4;
- marker.scale.z = 0.4;
- marker.color.a = 1.0;
- marker.color.r = 0.0;
- marker.color.g = 1.0;
- marker.color.b = 0.0;
- vis_pub_.publish(marker);
- }
-
- bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
- //make sure to set the plan to be empty initially
- plan.clear();
-
- //since this gets called on handle activate
- if(planner_costmap_ros_ == NULL)
- return false;
-
- //get the starting pose of the robot
- tf::Stamped<tf::Pose> global_pose;
- if(!planner_costmap_ros_->getRobotPose(global_pose))
- return false;
-
- geometry_msgs::PoseStamped start;
- tf::poseStampedTFToMsg(global_pose, start);
-
- //if the planner fails or returns a zero length plan, planning failed
- if(!planner_->makePlan(start, goal, plan) || plan.empty()){
- ROS_DEBUG("Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
- return false;
- }
-
- //we'll also push the goal point onto the end of the plan to make sure orientation is taken into account
- geometry_msgs::PoseStamped goal_copy = goal;
- goal_copy.header.stamp = ros::Time::now();
- plan.push_back(goal_copy);
-
- return true;
- }
-
- void MoveBase::rotateRobot(){
- ros::Rate r(controller_frequency_);
- //we'll perform two 180 degree in-place rotations
- for(unsigned int i = 0; i < 2; ++i){
- ROS_DEBUG("180 rotation %d", i);
- double angle = M_PI; //rotate 180 degrees
- tf::Stamped<tf::Pose> rotate_goal = tf::Stamped<tf::Pose>(tf::Pose(tf::Quaternion(angle, 0.0, 0.0), tf::Point(0.0, 0.0, 0.0)), ros::Time(), robot_base_frame_);
- geometry_msgs::PoseStamped rotate_goal_msg;
-
- try{
- tf_.transformPose(global_frame_, rotate_goal, rotate_goal);
- }
- catch(tf::TransformException& ex){
- ROS_ERROR("This tf error should never happen: %s", ex.what());
- return;
-
- }
-
- poseStampedTFToMsg(rotate_goal, rotate_goal_msg);
- std::vector<geometry_msgs::PoseStamped> rotate_plan;
- rotate_plan.push_back(rotate_goal_msg);
-
- //pass the rotation goal to the controller
- if(!tc_->setPlan(rotate_plan)){
- ROS_ERROR("Failed to pass global plan to the controller, aborting in place rotation attempt.");
- return;
- }
-
- geometry_msgs::Twist cmd_vel;
- while(!isPreemptRequested() && ros_node_.ok() && !tc_->isGoalReached()){
- if(tc_->computeVelocityCommands(cmd_vel)){
- //make sure that we send the velocity command to the base
- vel_pub_.publish(cmd_vel);
- ROS_DEBUG("Velocity commands produced by controller: vx: %.2f, vy: %.2f, vth: %.2f", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
- }
- else{
- //if we can't perform an in-place rotation then we'll just return
- return;
- }
- //make sure to sleep in the meantime
- r.sleep();
-
- }
- }
- }
-
- void MoveBase::publishZeroVelocity(){
- geometry_msgs::Twist cmd_vel;
- cmd_vel.linear.x = 0.0;
- cmd_vel.linear.y = 0.0;
- cmd_vel.angular.z = 0.0;
- vel_pub_.publish(cmd_vel);
-
- }
-
- robot_actions::ResultStatus MoveBase::execute(const geometry_msgs::PoseStamped& goal, geometry_msgs::PoseStamped& feedback){
- //if we shutdown our costmaps when we're deactivated... we need to start them back up now
- if(shutdown_costmaps_){
- planner_costmap_ros_->start();
- controller_costmap_ros_->start();
- }
-
- //on activation... we'll reset our costmaps
- clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
-
- //publish the goal point to the visualizer
- publishGoal(goal);
-
- std::vector<geometry_msgs::PoseStamped> global_plan;
- geometry_msgs::Twist cmd_vel;
- ros::Time last_valid_plan, last_valid_control;
-
- last_valid_control = ros::Time::now();
- last_valid_plan = ros::Time::now();
-
- ros::Rate r(controller_frequency_);
-
- ROS_DEBUG("move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
-
- //we'll loop while the node is up and while the action is not pre-empted
- while(!isPreemptRequested() && ros_node_.ok()){
- //for timing that gives real time even in simulation
- struct timeval start, end;
- double start_t, end_t, t_diff;
- gettimeofday(&start, NULL);
-
- //update feedback to correspond to our curent position
- tf::Stamped<tf::Pose> global_pose;
- planner_costmap_ros_->getRobotPose(global_pose);
- tf::poseStampedTFToMsg(global_pose, feedback);
-
- //push the feedback out
- update(feedback);
-
- //check that the observation buffers for the costmap are current, we don't want to drive blind
- if(!controller_costmap_ros_->isCurrent()){
- ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
- publishZeroVelocity();
- r.sleep();
- continue;
- }
-
- //the move_base state machine, handles the control logic for navigation
- switch(state_){
- //if we are in a planning state, then we'll attempt to make a plan
- case PLANNING:
- ROS_DEBUG("In planning state");
- if(makePlan(goal, global_plan)){
- if(!tc_->setPlan(global_plan)){
- //ABORT and SHUTDOWN COSTMAPS
- ROS_ERROR("Failed to pass global plan to the controller, aborting.");
- resetState();
- return robot_actions::ABORTED;
- }
- last_valid_plan = ros::Time::now();
- state_ = CONTROLLING;
-
- //make sure to reset clearing state since we were able to find a valid plan
- clearing_state_ = CONSERVATIVE_RESET;
-
- publishZeroVelocity();
- }
- else{
- ros::Time attempt_end = last_valid_plan + ros::Duration(planner_patience_);
-
- //check if we've tried to make a plan for over our time limit
- if(ros::Time::now() > attempt_end){
- //we'll move into our obstacle clearing mode
- state_ = CLEARING;
- publishZeroVelocity();
- }
- }
- break;
-
- //if we're controlling, we'll attempt to find valid velocity commands
- case CONTROLLING:
- ROS_DEBUG("In controlling state");
-
- //check to see if we've reached our goal
- if(tc_->isGoalReached()){
- ROS_DEBUG("Goal reached!");
- resetState();
- return robot_actions::SUCCESS;
- }
-
- if(tc_->computeVelocityCommands(cmd_vel)){
- last_valid_control = ros::Time::now();
- //make sure that we send the velocity command to the base
- vel_pub_.publish(cmd_vel);
- }
- else {
- ros::Time attempt_end = last_valid_control + ros::Duration(controller_patience_);
-
- //check if we've tried to find a valid control for longer than our time limit
- if(ros::Time::now() > attempt_end){
- ROS_ERROR("Aborting because of failure to find a valid control for %.2f seconds", controller_patience_);
- resetState();
- return robot_actions::ABORTED;
- }
-
- //otherwise, if we can't find a valid control, we'll go back to planning
- last_valid_plan = ros::Time::now();
- state_ = PLANNING;
- publishZeroVelocity();
- }
-
- break;
-
- //we'll try to clear out space with the following actions
- case CLEARING:
- switch(clearing_state_){
- //first, we'll try resetting the costmaps conservatively to see if we find a plan
- case CONSERVATIVE_RESET:
- ROS_DEBUG("In conservative reset state");
- resetCostmaps(conservative_reset_dist_, conservative_reset_dist_);
- clearing_state_ = IN_PLACE_ROTATION;
- state_ = PLANNING;
- break;
- //next, we'll try an in-place rotation to try to clear out space
- case IN_PLACE_ROTATION:
- ROS_DEBUG("In in-place rotation state");
- rotateRobot();
- clearing_state_ = AGGRESSIVE_RESET;
- state_ = PLANNING;
- break;
- //finally, we'll try resetting the costmaps aggresively to clear out space
- ...
[truncated message content] |
