Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11496] pkg/trunk
|
From: <tpr...@us...> - 2009-02-21 01:06:03
|
Revision: 11496
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11496&view=rev
Author: tpratkanis
Date: 2009-02-21 01:05:58 +0000 (Sat, 21 Feb 2009)
Log Message:
-----------
Changing highlevel_controllers to use the new state storage method for milestone2. The three booleans (active, aborted, preempted) have been compressed into a single status flag.
Modified Paths:
--------------
pkg/trunk/highlevel/executive_python/src/movearm_adapter.py
pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
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/highlevel_controllers/highlevel_controller.hh
pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
pkg/trunk/pr2_msgs/msg/MoveArmState.msg
pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg
Modified: pkg/trunk/highlevel/executive_python/src/movearm_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/movearm_adapter.py 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/executive_python/src/movearm_adapter.py 2009-02-21 01:05:58 UTC (rev 11496)
@@ -65,7 +65,7 @@
self.last_plan_time = rospy.get_time()
def active(self):
- return self.state.active == 1
+ return self.state.active == self.state.ACTIVE
#Have we reached a goal
def goalReached(self):
Modified: pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-02-21 01:05:58 UTC (rev 11496)
@@ -63,7 +63,7 @@
self.last_plan_time = rospy.get_time()
def active(self):
- return self.state.active == 1
+ return self.state.status == self.state.ACTIVE
#Have we reached a goal
def goalReached(self):
Modified: pkg/trunk/highlevel/executive_python/src/recharge_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/recharge_adapter.py 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/executive_python/src/recharge_adapter.py 2009-02-21 01:05:58 UTC (rev 11496)
@@ -69,4 +69,4 @@
return self.state.done == 1
def charging(self):
- return self.state.active == 1
+ return self.state.status == self.state.ACTIVE
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh 2009-02-21 01:05:58 UTC (rev 11496)
@@ -42,12 +42,12 @@
if(lastUpdated == getCurrentTick() && state != UNDEFINED)
return;
- if(stateMsg.active && state != ACTIVE){
+ if(stateMsg.status == stateMsg.ACTIVE && state != ACTIVE){
state = ACTIVE;
lastUpdated = getCurrentTick();
ROS_DEBUG("Received transition to ACTIVE");
}
- else if(!stateMsg.active && state != INACTIVE){
+ else if(stateMsg.status != stateMsg.ACTIVE && state != INACTIVE){
state = INACTIVE;
lastUpdated = getCurrentTick();
ROS_DEBUG("Received transition to INACTIVE");
@@ -83,8 +83,8 @@
if(state == INACTIVE){
obs = new ObservationByValue(timelineName, inactivePredicate);
fillInactiveObservationParameters(obs);
- obs->push_back("aborted", new BoolDomain(stateMsg.aborted));
- obs->push_back("preempted", new BoolDomain(stateMsg.preempted));
+ obs->push_back("aborted", new BoolDomain(stateMsg.status == stateMsg.ABORTED));
+ obs->push_back("preempted", new BoolDomain(stateMsg.status == stateMsg.PREEMPTED));
}
else {
obs = new ObservationByValue(timelineName, activePredicate);
@@ -132,8 +132,7 @@
// synchronizing
if(!enableController){
stateMsg.lock();
- stateMsg.active = false;
- stateMsg.preempted = true;
+ stateMsg.status = stateMsg.PREEMPTED;
handleCallback();
stateMsg.unlock();
}
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh 2009-02-21 01:05:58 UTC (rev 11496)
@@ -172,14 +172,13 @@
ROS_DEBUG("Last plan at %f seconds:", lastPlan.toSec());
ROS_DEBUG("Elapsed time is %f seconds:", elapsed_time.toSec());
if ((elapsed_time > timeout) && timeout.toSec() != 0.0) {
- this->stateMsg.aborted = 1;
+ this->stateMsg.status = this->stateMsg.ABORTED;
ROS_INFO("Controller timed out.");
deactivate();
}
handlePlanningFailure();
unlock();
} else {
- this->stateMsg.aborted = 0;
lastPlan = ros::Time::now();
}
}
@@ -207,19 +206,18 @@
bool isValid() {
return this->stateMsg.valid;
}
-
/**
* @brief Access aborted state of the planner.
*/
bool isAborted() {
- return this->stateMsg.aborted;
+ return this->stateMsg.status == this->stateMsg.ABORTED;
}
/**
* @brief Access preempted state of the planner.
*/
bool isPreempted() {
- return this->stateMsg.preempted;
+ return this->stateMsg.status == this->stateMsg.PREEMPTED;
}
/**
@@ -230,11 +228,8 @@
ROS_INFO("Activating controller with timeout of %f seconds\n", this->goalMsg.timeout);
this->state = ACTIVE;
- this->stateMsg.active = 1;
- this->stateMsg.valid = 0;
+ this->stateMsg.status = this->stateMsg.ACTIVE;
this->stateMsg.done = 0;
- this->stateMsg.aborted = 0;
- this->stateMsg.preempted = 0;
handleActivation();
}
@@ -246,7 +241,9 @@
ROS_INFO("Deactivating controller\n");
this->state = INACTIVE;
- this->stateMsg.active = 0;
+ if (this->stateMsg.status == this->stateMsg.ACTIVE) {
+ this->stateMsg.status = this->stateMsg.INACTIVE;
+ }
this->stateMsg.valid = 0;
handleDeactivation();
@@ -376,7 +373,7 @@
}
else if(state == ACTIVE){
ROS_INFO("Controller preempted.");
- this->stateMsg.preempted = 1;
+ this->stateMsg.status = this->stateMsg.PREEMPTED;
deactivate();
// If we are active, and this is a goal, publish the state message and activate. This allows us
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-02-21 01:05:58 UTC (rev 11496)
@@ -1,20 +1,27 @@
Header header
-# Is the controller actively going to a goal?
-byte active
+#Status options
+byte INACTIVE=0
+byte ACTIVE=1
+byte ABORTED=2
+byte PREEMPTED=3
+# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
+byte status
+
+#Id of the goal
+int32 goal_id
+
+#Comment for debug
+string comment
+
+
# Did the controller find a valid path?
byte valid
# 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
Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-02-21 01:05:58 UTC (rev 11496)
@@ -1,14 +1,25 @@
Header header
-#Is the planner/controller actively trying to achieve a goal
-byte active
+
+
+#Status options
+byte INACTIVE=0
+byte ACTIVE=1
+byte ABORTED=2
+byte PREEMPTED=3
+
+# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
+byte status
+
+#Id of the goal
+int32 goal_id
+
+#Comment for debug
+string comment
+
+#Did we reach the goal?
+byte done
#Did the planner/controller find a valid way to achieve the goal
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/MoveEndEffectorState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-02-21 01:05:58 UTC (rev 11496)
@@ -1,14 +1,24 @@
Header header
-#Is the planner/controller actively trying to achieve a goal
-byte active
+
+#Status options
+byte INACTIVE=0
+byte ACTIVE=1
+byte ABORTED=2
+byte PREEMPTED=3
+
+# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
+byte status
+
+#Id of the goal
+int32 goal_id
+
+#Comment for debug
+string comment
+
#Did the planner/controller find a valid way to achieve the goal
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/prcore/robot_msgs/msg/Planner2DState.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg 2009-02-21 01:05:58 UTC (rev 11496)
@@ -1,14 +1,25 @@
Header header
-# Is the planner actively going to a goal?
-byte active
+
+
+#Status options
+byte INACTIVE=0
+byte ACTIVE=1
+byte ABORTED=2
+byte PREEMPTED=3
+
+# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
+byte status
+
+#Id of the goal
+int32 goal_id
+
+#Comment for debug
+string comment
+
# Did the planner find a valid path?
byte valid
# Have we arrived at the goal?
byte done
-#Did the planner give up?
-byte aborted
-#Was the planner told to stop?
-byte preempted
# Current location (m,m,rad)
deprecated_msgs/Pose2DFloat32 pos
# Goal location (m,m,rad)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
