Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21723] pkg/trunk/stacks
|
From: <vij...@us...> - 2009-08-12 22:52:33
|
Revision: 21723
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21723&view=rev
Author: vijaypradeep
Date: 2009-08-12 22:52:19 +0000 (Wed, 12 Aug 2009)
Log Message:
-----------
Adding a blocking execute call to SingleActionGoalServer. Porting move_base to the blocking execute
Modified Paths:
--------------
pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h 2009-08-12 22:47:31 UTC (rev 21722)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h 2009-08-12 22:52:19 UTC (rev 21723)
@@ -37,6 +37,7 @@
#ifndef ACTIONLIB_SINGLE_GOAL_ACTION_SERVER_H_
#define ACTIONLIB_SINGLE_GOAL_ACTION_SERVER_H_
+#include <boost/thread/condition.hpp>
#include <ros/ros.h>
#include <actionlib/server/action_server.h>
#include <actionlib/action_definition.h>
@@ -59,14 +60,20 @@
ACTION_DEFINITION(ActionSpec);
typedef typename ActionServer<ActionSpec>::GoalHandle GoalHandle;
+ typedef boost::function<void (const GoalConstPtr&)> ExecuteCallback;
/**
* @brief Constructor for a SingleGoalActionServer
* @param n A NodeHandle to create a namespace under
* @param name A name for the action server
+ * @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.
*/
- SingleGoalActionServer(ros::NodeHandle n, std::string name);
+ SingleGoalActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = NULL);
+ ~SingleGoalActionServer();
+
/**
* @brief Accepts a new goal when one is available The status of this
* goal is set to active upon acceptance, and the status of any
@@ -140,6 +147,13 @@
*/
void preemptCallback(GoalHandle preempt);
+ /**
+ * @brief Called from a separate thread to call blocking execute calls
+ */
+ void executeLoop();
+
+ ros::NodeHandle n_;
+
boost::shared_ptr<ActionServer<ActionSpec> > as_;
GoalHandle current_goal_, next_goal_;
@@ -150,7 +164,10 @@
boost::function<void ()> goal_callback_;
boost::function<void ()> preempt_callback_;
+ ExecuteCallback execute_callback_;
+ boost::condition execute_condition_;
+ boost::thread* execute_thread_;
};
};
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h 2009-08-12 22:47:31 UTC (rev 21722)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h 2009-08-12 22:52:19 UTC (rev 21723)
@@ -38,17 +38,32 @@
#define ACTION_LIB_SINGLE_GOAL_ACTION_SERVER_H_
namespace actionlib {
template <class ActionSpec>
- SingleGoalActionServer<ActionSpec>::SingleGoalActionServer(ros::NodeHandle n, std::string name)
- : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false) {
+ SingleGoalActionServer<ActionSpec>::SingleGoalActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback)
+ : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback) {
- //create the action server
- as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
- boost::bind(&SingleGoalActionServer::goalCallback, this, _1),
- boost::bind(&SingleGoalActionServer::preemptCallback, this, _1)));
+ //create the action server
+ as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
+ boost::bind(&SingleGoalActionServer::goalCallback, this, _1),
+ boost::bind(&SingleGoalActionServer::preemptCallback, this, _1)));
+ if (execute_callback_ != NULL)
+ {
+ execute_thread_ = new boost::thread(boost::bind(&SingleGoalActionServer::executeLoop, this));
}
+ }
template <class ActionSpec>
+ SingleGoalActionServer<ActionSpec>::~SingleGoalActionServer()
+ {
+ if (execute_callback_)
+ {
+ assert(execute_thread_);
+ execute_thread_->join();
+ delete execute_thread_;
+ }
+ }
+
+ template <class ActionSpec>
boost::shared_ptr<const typename SingleGoalActionServer<ActionSpec>::Goal> SingleGoalActionServer<ActionSpec>::acceptNewGoal(){
boost::recursive_mutex::scoped_lock lock(lock_);
@@ -120,7 +135,11 @@
template <class ActionSpec>
void SingleGoalActionServer<ActionSpec>::registerGoalCallback(boost::function<void ()> cb){
- goal_callback_ = cb;
+ // Cannot register a goal callback if an execute callback exists
+ if (execute_callback_)
+ ROS_WARN("Cannot call SingleGoalActionServer::registerGoalCallback() because an executeCallback exists. Not going to register it.");
+ else
+ goal_callback_ = cb;
}
template <class ActionSpec>
@@ -149,6 +168,9 @@
//if the user has defined a goal callback, we'll call it now
if(goal_callback_)
goal_callback_();
+
+ // Trigger runLoop to call execute()
+ execute_condition_.notify_all();
}
else{
//the goal requested has already been preempted by a different goal, so we're not going to execute it
@@ -176,5 +198,40 @@
new_goal_preempt_request_ = true;
}
}
+
+ template <class ActionSpec>
+ void SingleGoalActionServer<ActionSpec>::executeLoop(){
+
+ ros::Duration loop_duration = ros::Duration().fromSec(.1);
+
+ while (n_.ok())
+ {
+ boost::recursive_mutex::scoped_lock lock(lock_);
+ if (isActive())
+ ROS_ERROR("Should never reach this code with an active goal");
+ else if (isNewGoalAvailable())
+ {
+ GoalConstPtr goal = acceptNewGoal();
+
+ ROS_FATAL_COND(!execute_callback_, "execute_callback_ must exist. This is a bug in SingleGoalActionServer");
+
+ // Make sure we're not locked when we call execute
+ lock.unlock();
+ execute_callback_(goal);
+ lock.lock();
+
+ if (isActive())
+ {
+ ROS_WARN("Your executeCallback did not set the goal to a terminal status.\n"
+ "This is a bug in your ActionServer implementation. Fix your code!\n"
+ "For now, the ActionServer will set this goal to aborted");
+ setAborted();
+ }
+ }
+ else
+ execute_condition_.timed_wait(lock, boost::posix_time::milliseconds(loop_duration.toSec() * 1000.0f));
+ }
+ }
+
};
#endif
Modified: pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-12 22:47:31 UTC (rev 21722)
+++ pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-12 22:52:19 UTC (rev 21723)
@@ -99,11 +99,6 @@
*/
void executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan);
- /**
- * @brief The main loop of the MoveBase action
- */
- void runLoop();
-
private:
/**
* @brief A service call that can be made when the action is inactive that will return a plan
@@ -160,6 +155,8 @@
void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
+ void executeCb(const MoveBaseGoalConstPtr& move_base_goal);
+
geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
ros::NodeHandle ros_node_;
@@ -186,10 +183,9 @@
ClearingState clearing_state_;
ros::Time last_valid_plan_, last_valid_control_;
- boost::thread* run_loop_;
- pluginlib::PluginLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
- pluginlib::PluginLoader<nav_core::BaseLocalPlanner> blp_loader_;
-
+ pluginlib::PluginLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
+ pluginlib::PluginLoader<nav_core::BaseLocalPlanner> blp_loader_;
+
};
};
#endif
Modified: pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-12 22:47:31 UTC (rev 21722)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-12 22:52:19 UTC (rev 21723)
@@ -42,8 +42,8 @@
MoveBase::MoveBase(std::string name, tf::TransformListener& tf) :
tf_(tf),
- as_(ros::NodeHandle(), name),
- tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
+ as_(ros::NodeHandle(), name, boost::bind(&MoveBase::executeCb, this, _1)),
+ tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
planner_(NULL), bgp_loader_("nav_core", "BaseGlobalPlanner"),
blp_loader_("nav_core", "BaseLocalPlanner"){
@@ -125,8 +125,6 @@
//the initial clearing state will be to conservatively clear the costmaps
clearing_state_ = CONSERVATIVE_RESET;
- run_loop_ = new boost::thread(boost::bind(&MoveBase::runLoop, this));
-
}
void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
@@ -216,13 +214,13 @@
//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
+ //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;
+ 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;
@@ -252,10 +250,6 @@
}
MoveBase::~MoveBase(){
- if(run_loop_){
- run_loop_->join();
- delete run_loop_;
- }
if(planner_ != NULL)
delete planner_;
@@ -369,13 +363,13 @@
//just get the latest available transform... for accuracy they should send
//goals in the frame of the planner
- goal_pose.stamp_ = ros::Time();
+ goal_pose.stamp_ = ros::Time();
try{
tf_.transformPose(global_frame, goal_pose, global_pose);
}
catch(tf::TransformException& ex){
- ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
+ ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
goal_pose.frame_id_.c_str(), global_frame.c_str(), ex.what());
return goal_pose_msg;
}
@@ -386,81 +380,67 @@
}
- void MoveBase::runLoop(){
- ros::Rate r(controller_frequency_);
- geometry_msgs::PoseStamped goal;
+ void MoveBase::executeCb(const MoveBaseGoalConstPtr& move_base_goal)
+ {
+ geometry_msgs::PoseStamped goal = move_base_goal->target_pose;
std::vector<geometry_msgs::PoseStamped> global_plan;
- while(ros_node_.ok()){
- if(as_.isActive()){
- if(!as_.isPreemptRequested()){
- if(as_.isNewGoalAvailable()){
- //if we're active and a new goal is available, we'll accept it, but we won't shut anything down
- goal = goalToGlobalFrame((*as_.acceptNewGoal()).target_pose);
- //we'll make sure that we reset our state for the next execution cycle
- clearing_state_ = CONSERVATIVE_RESET;
- state_ = PLANNING;
+ ros::Rate r(controller_frequency_);
- //publish the goal point to the visualizer
- ROS_DEBUG("move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
- publishGoal(goal);
+ while(as_.isActive())
+ {
+ if (!ros_node_.ok())
+ {
+ as_.setAborted();
+ break;
+ }
- //make sure to reset our timeouts
- last_valid_control_ = ros::Time::now();
- last_valid_plan_ = ros::Time::now();
- }
- //for timing that gives real time even in simulation
- struct timeval start, end;
- double start_t, end_t, t_diff;
- gettimeofday(&start, NULL);
+ if(!as_.isPreemptRequested()){
+ if(as_.isNewGoalAvailable()){
+ //if we're active and a new goal is available, we'll accept it, but we won't shut anything down
+ goal = goalToGlobalFrame((*as_.acceptNewGoal()).target_pose);
- //the real work on pursuing a goal is done here
- executeCycle(goal, global_plan);
-
- gettimeofday(&end, NULL);
- start_t = start.tv_sec + double(start.tv_usec) / 1e6;
- end_t = end.tv_sec + double(end.tv_usec) / 1e6;
- t_diff = end_t - start_t;
- ROS_DEBUG("Full control cycle time: %.9f\n", t_diff);
- }
- else{
- //if we've been preempted explicitly we need to shut things down
- resetState();
+ //we'll make sure that we reset our state for the next execution cycle
+ clearing_state_ = CONSERVATIVE_RESET;
+ state_ = PLANNING;
- //notify the ActionServer that we've successfully preemted
- ROS_DEBUG("Move base preempting the current goal");
- as_.setPreempted();
- }
- }
- else if(as_.isNewGoalAvailable()){
- //if we're inactive and a new goal is available, we'll accept it
- goal = goalToGlobalFrame((*as_.acceptNewGoal()).target_pose);
+ //publish the goal point to the visualizer
+ ROS_DEBUG("move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
+ publishGoal(goal);
- //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();
+ //make sure to reset our timeouts
+ last_valid_control_ = ros::Time::now();
+ last_valid_plan_ = ros::Time::now();
}
+ //for timing that gives real time even in simulation
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
- //on activation... we'll reset our costmaps
- clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
+ //the real work on pursuing a goal is done here
+ executeCycle(goal, global_plan);
- //publish the goal point to the visualizer
- ROS_DEBUG("move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
- publishGoal(goal);
+ gettimeofday(&end, NULL);
+ start_t = start.tv_sec + double(start.tv_usec) / 1e6;
+ end_t = end.tv_sec + double(end.tv_usec) / 1e6;
+ t_diff = end_t - start_t;
+ ROS_DEBUG("Full control cycle time: %.9f\n", t_diff);
+ }
+ else{
+ //if we've been preempted explicitly we need to shut things down
+ resetState();
- //make sure to reset our timeouts
- last_valid_control_ = ros::Time::now();
- last_valid_plan_ = ros::Time::now();
+ //notify the ActionServer that we've successfully preemted
+ ROS_DEBUG("Move base preempting the current goal");
+ as_.setPreempted();
}
//make sure to sleep for the remainder of our cycle time
if(!r.sleep() && state_ == CONTROLLING)
ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
-
}
}
-
+
void MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
//we need to be able to publish velocity commands
geometry_msgs::Twist cmd_vel;
@@ -474,7 +454,7 @@
//push the feedback out
position_pub_.publish(current_position);
- //check that the observation buffers for the costmap are current, we don't want to drive blind
+ //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();
@@ -505,7 +485,7 @@
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
+ //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;
@@ -526,7 +506,7 @@
resetState();
as_.setSucceeded();
return;
- }
+ }
if(tc_->computeVelocityCommands(cmd_vel)){
last_valid_control_ = ros::Time::now();
@@ -542,7 +522,7 @@
resetState();
as_.setAborted();
return;
- }
+ }
//otherwise, if we can't find a valid control, we'll go back to planning
last_valid_plan_ = ros::Time::now();
@@ -629,7 +609,7 @@
- }
+ }
void MoveBase::resetState(){
state_ = PLANNING;
@@ -653,12 +633,12 @@
int main(int argc, char** argv){
ros::init(argc, argv, "move_base");
tf::TransformListener tf(ros::Duration(10));
-
+
move_base::MoveBase move_base(ros::this_node::getName(), tf);
//ros::MultiThreadedSpinner s;
ros::spin();
-
+
return(0);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
