Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9281] pkg/trunk
|
From: <tpr...@us...> - 2009-01-12 23:39:45
|
Revision: 9281
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9281&view=rev
Author: tpratkanis
Date: 2009-01-12 23:39:40 +0000 (Mon, 12 Jan 2009)
Log Message:
-----------
Add abort and prempt functionality to highlevel controllers.
Modified Paths:
--------------
pkg/trunk/highlevel/executive_python/src/recharge_adapter.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh
pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
pkg/trunk/highlevel/highlevel_controllers/msg/PlugInGoal.msg
pkg/trunk/highlevel/highlevel_controllers/msg/PlugInState.msg
pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg
pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
pkg/trunk/pr2_msgs/msg/MoveArmState.msg
pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg
pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
pkg/trunk/robot_msgs/msg/Planner2DGoal.msg
Modified: pkg/trunk/highlevel/executive_python/src/recharge_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/recharge_adapter.py 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/executive_python/src/recharge_adapter.py 2009-01-12 23:39:40 UTC (rev 9281)
@@ -52,6 +52,7 @@
goal = RechargeGoal()
goal.recharge_level = self.recharge_level
goal.enable = 1
+ goal.timeout = 0
goal.pose.x = pose[0]
goal.pose.y = pose[1]
goal.pose.th = pose[2]
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh 2009-01-12 23:39:40 UTC (rev 9281)
@@ -114,6 +114,7 @@
G goalMsg;
fillRequestParameters(goalMsg, goal);
goalMsg.enable = enableController;
+ goalMsg.timeout = 0;
ROS_DEBUG("[%d}Dispatching %s", getCurrentTick(), goal->toString().c_str());
m_node->publishMsg<G>(goalTopic, goalMsg);
Modified: pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2009-01-12 23:39:40 UTC (rev 9281)
@@ -69,8 +69,8 @@
* @param goalTopic The ROS topic on which controller goals are received
*/
HighlevelController(const std::string& nodeName, const std::string& _stateTopic, const std::string& _goalTopic):
- ros::node(nodeName), initialized(false), terminated(false), stateTopic(_stateTopic), goalTopic(_goalTopic),
- controllerCycleTime_(0.1), plannerCycleTime_(0.0), plannerThread_(NULL){
+ ros::node(nodeName), initialized(false), terminated(false), stateTopic(_stateTopic),
+ goalTopic(_goalTopic), controllerCycleTime_(0.1), plannerCycleTime_(0.0), plannerThread_(NULL), timeout(0, 0) {
// Obtain the control frequency for this node
double controller_frequency(10);
@@ -146,6 +146,7 @@
* it will call the planner with the given timeout.
*/
void plannerLoop(){
+ ros::Time lastPlan = ros::Time::now();
while(ok() && !isTerminated()){
ros::Time curr = ros::Time::now();
@@ -156,9 +157,16 @@
setValid(makePlan());
if(!isValid()){
// Could use a refined locking scheme but for now do not want to delegate that to a derived class
- lock();
+ lock();
+ if ((ros::Time::now() - lastPlan) < timeout && timeout.toSec() != 0.0) {
+ this->stateMsg.aborted = true;
+ ROS_ERROR("Controller timed out.");
+ }
handlePlanningFailure();
- unlock();
+ unlock();
+ } else {
+ this->stateMsg.aborted = false;
+ lastPlan = ros::Time::now();
}
}
}
@@ -187,6 +195,20 @@
}
/**
+ * @brief Access aborted state of the planner.
+ */
+ bool isAborted() {
+ return this->stateMsg.aborted;
+ }
+
+ /**
+ * @brief Access preempted state of the planner.
+ */
+ bool isPreempted() {
+ return this->stateMsg.preempted;
+ }
+
+ /**
* @brief Activation of the controller will set the state, the stateMsg but indicate that the
* goal has not yet been accomplished and that no plan has been constructed yet.
*/
@@ -197,6 +219,8 @@
this->stateMsg.active = 1;
this->stateMsg.valid = 0;
this->stateMsg.done = 0;
+ this->stateMsg.aborted = 0;
+ this->stateMsg.preempted = 0;
handleActivation();
}
@@ -207,9 +231,15 @@
void deactivate(){
ROS_INFO("Deactivating controller\n");
+ if (this->state == ACTIVE) {
+ ROS_INFO("Controller preempted.");
+ this->stateMsg.preempted = 1;
+ }
+
this->state = INACTIVE;
this->stateMsg.active = 0;
this->stateMsg.valid = 0;
+ this->stateMsg.aborted = 0;
handleDeactivation();
}
@@ -320,6 +350,13 @@
private:
void goalCallback(){
+ if (goalMsg.timeout < 0) {
+ ROS_ERROR("Controller was given negative timeout, setting to zero.");
+ timeout = ros::Duration().fromSec(0);
+ } else {
+ timeout = ros::Duration().fromSec(goalMsg.timeout);
+ }
+
// Do nothing if not initialized
if(!isInitialized() || isTerminated())
return;
@@ -432,6 +469,7 @@
boost::mutex lock_; /*!< Lock for access to class members in callbacks */
boost::thread* plannerThread_; /*!< Thread running the planner loop */
highlevel_controllers::Ping shutdownMsg_; /*!< For receiving shutdown from executive */
+ ros::Duration timeout; /*< The time limit for planning failure. */
};
#endif
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/PlugInGoal.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/PlugInGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/PlugInGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -3,3 +3,7 @@
byte goal
#True to submit a goal, false to recall it
byte enable
+
+# Timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
+float64 timeout
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/PlugInState.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/PlugInState.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/PlugInState.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -7,5 +7,9 @@
byte done
# Are we waiting to be plugged in or unplugged. 1 if plug in.
byte goal
+#Did the planner give up?
+byte aborted
+#Was the planner told to stop?
+byte preempted
# Are we plugged in or unplugged. 1 if plug in.
byte state
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -10,3 +10,7 @@
# True to submit a goal, false to recall it
byte enable
+
+# Timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
+float64 timeout
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -9,6 +9,12 @@
# Have we arrived at the goal?
byte done
+#Did the planner give up?
+byte aborted
+
+#Was the planner told to stop?
+byte preempted
+
# Actual recharge level
float32 recharge_level
@@ -16,4 +22,4 @@
float32 goal_recharge_level
# The goal pose for accessing the plug
-std_msgs/Pose2DFloat32 goal_pose
\ No newline at end of file
+std_msgs/Pose2DFloat32 goal_pose
Modified: pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -4,3 +4,7 @@
# Indicate if the goal is being enabled or disabled
byte enable
+
+# Total timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
+float64 timeout
Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -5,6 +5,10 @@
byte valid
#Did we actually successfully arrive at the goal?
byte done
+#Did the planner give up?
+byte aborted
+#Was the planner told to stop?
+byte preempted
#Current arm configuration
robot_msgs/JointState[] configuration
#Goal arm configuration
Modified: pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -4,3 +4,7 @@
# Indicate if the goal is being enabled or disabled
byte enable
+
+# Timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
+float64 timeout
Modified: pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -5,6 +5,10 @@
byte valid
#Did we actually successfully arrive at the goal?
byte done
+#Did the planner give up?
+byte aborted
+#Was the planner told to stop?
+byte preempted
#Current arm configuration
robot_msgs/JointState[] configuration
#Goal arm configuration
Modified: pkg/trunk/robot_msgs/msg/Planner2DGoal.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Planner2DGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/robot_msgs/msg/Planner2DGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -2,5 +2,6 @@
std_msgs/Pose2DFloat32 goal
byte enable
-# Timelimit for planning. Set it to zero for unlimited.
+# Total timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
float64 timeout
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
