Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13808] pkg/trunk
|
From: <mc...@us...> - 2009-04-14 14:11:14
|
Revision: 13808
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13808&view=rev
Author: mcgann
Date: 2009-04-14 14:11:06 +0000 (Tue, 14 Apr 2009)
Log Message:
-----------
New action API
Modified Paths:
--------------
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_door.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h
pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_grasp_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ros_action_adapter.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/doors/stubs.launch
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_fine_outlet_detect.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_move_and_grasp_plug.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_stow_plug.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_untuck_arms.h
pkg/trunk/highlevel/plugs/plugs_core/src/action_fine_outlet_detect.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_move_and_grasp_plug.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_stow_plug.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_untuck_arms.cpp
pkg/trunk/highlevel/robot_actions/include/robot_actions/action.h
pkg/trunk/highlevel/robot_actions/include/robot_actions/message_adapter.h
pkg/trunk/highlevel/robot_actions/src/action_runner.cpp
pkg/trunk/highlevel/robot_actions/test/utest.cc
pkg/trunk/highlevel/safety/safety_core/include/safety_core/action_detect_plug_on_base.h
pkg/trunk/highlevel/safety/safety_core/include/safety_core/action_tuck_arms.h
pkg/trunk/highlevel/safety/safety_core/src/action_detect_plug_on_base.cpp
pkg/trunk/highlevel/safety/safety_core/src/action_runner.cpp
pkg/trunk/highlevel/safety/safety_core/src/action_tuck_arms.cpp
pkg/trunk/mechanism/mechanism_control/src/action_mechanism_control.cpp
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -50,18 +50,15 @@
{
public:
DetectDoorAction(ros::Node& node);
+
~DetectDoorAction();
- virtual void handleActivate(const robot_msgs::Door& door);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
robot_msgs::Door tmp_result_;
-
private:
bool laserDetection(const robot_msgs::Door& door_in, robot_msgs::Door& door_out);
-
- bool request_preempt_;
tf::TransformListener tf_;
};
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -52,16 +52,14 @@
DetectHandleAction(ros::Node& node);
~DetectHandleAction();
- virtual void handleActivate(const robot_msgs::Door& door);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
private:
bool laserDetection(const robot_msgs::Door& door_in, robot_msgs::Door& door_out);
bool cameraDetection(const robot_msgs::Door& door, robot_msgs::Door& door_out);
ros::Node& node_;
- bool request_preempt_;
tf::TransformListener tf_;
};
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_door.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_door.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -58,8 +58,7 @@
GraspDoorAction(ros::Node& node);
~GraspDoorAction();
- virtual void handleActivate(const robot_msgs::Door& door);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
private:
@@ -67,8 +66,6 @@
double getDoorAngle(const robot_msgs::Door& door);
ros::Node& node_;
-
- bool request_preempt_;
tf::TransformListener tf_;
std_srvs::Empty::Request req_empty;
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -56,15 +56,11 @@
OpenDoorAction();
~OpenDoorAction();
- virtual void handleActivate(const robot_msgs::Door& door);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
-
private:
ros::Node* node_;
- bool request_preempt_;
-
robot_msgs::TaskFrameFormalism tff_stop_, tff_handle_, tff_door_;
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -33,7 +33,6 @@
*********************************************************************/
#include <door_handle_detector/DoorsDetectorCloud.h>
-#include <door_handle_detector/door_functions.h>
#include <point_cloud_assembler/BuildCloudAngle.h>
#include "doors_core/action_detect_door.h"
@@ -57,38 +56,33 @@
DetectDoorAction::~DetectDoorAction(){};
-
-void DetectDoorAction::handleActivate(const robot_msgs::Door& door)
+robot_actions::ResultStatus DetectDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
{
- ROS_INFO("DetectDoorAction: handle activate");
- request_preempt_ = false;
- notifyActivated();
+ ROS_INFO("DetectDoorAction: execute");
+ feedback = goal;
robot_msgs::Door result_laser;
- if (!laserDetection(door, result_laser)){
- if (request_preempt_){
+ if (!laserDetection(goal, result_laser)){
+ if (isPreemptRequested()){
ROS_ERROR("DetectDoorAction: Preempted");
- notifyPreempted(door);
+ return robot_actions::PREEMPTED;
}
else{
ROS_ERROR("DetectDoorAction: Aborted");
- notifyAborted(door);
+ return robot_actions::ABORTED;
}
}
- else{
- ROS_INFO("DetectDoorAction: Succeeded");
- notifySucceeded(result_laser);
- tmp_result_ = result_laser;
- }
+
+ ROS_INFO("DetectDoorAction: Succeeded");
+ feedback = result_laser;
+ return robot_actions::SUCCESS;
}
-
-
-
bool DetectDoorAction::laserDetection(const robot_msgs::Door& door_in, robot_msgs::Door& door_out)
{
// check where robot is relative to door
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
+
if (!tf_.canTransform("base_footprint", "laser_tilt_link", ros::Time(), ros::Duration().fromSec(5.0))){
ROS_ERROR("DetectDoorAction: error getting transform from 'base_footprint' to 'laser_tilt_link'");
return false;
@@ -100,11 +94,14 @@
(door_in.frame_p1.y+door_in.frame_p2.y)/2.0,
(door_in.frame_p1.z+door_in.frame_p2.z)/2.0),
ros::Time(), door_in.header.frame_id);
- if (request_preempt_) return false;
+
+ if (isPreemptRequested()) return false;
+
if (!tf_.canTransform("base_footprint", doorpoint.frame_id_, ros::Time(), ros::Duration().fromSec(5.0))){
ROS_ERROR("DetectDoorAction: error getting transform from 'base_footprint' to '%s'", doorpoint.frame_id_.c_str());
return false;
}
+
tf_.transformPoint("base_footprint", doorpoint, doorpoint);
double dist = doorpoint[0];
double door_bottom = -0.5;
@@ -112,7 +109,8 @@
ROS_INFO("DetectDoorAction: tilt laser is at height %f, and door at distance %f", laser_height, dist);
// gets a point cloud from the point_cloud_srv
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
+
ROS_INFO("DetectDoorAction: get a point cloud from the door");
point_cloud_assembler::BuildCloudAngle::Request req_pointcloud;
point_cloud_assembler::BuildCloudAngle::Response res_pointcloud;
@@ -125,26 +123,20 @@
}
// detect door
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
+
ROS_INFO("DetectDoorAction: detect the door");
door_handle_detector::DoorsDetectorCloud::Request req_doordetect;
door_handle_detector::DoorsDetectorCloud::Response res_doordetect;
req_doordetect.door = door_in;
req_doordetect.cloud = res_pointcloud.cloud;
+
if (!ros::service::call("doors_detector_cloud", req_doordetect, res_doordetect)){
ROS_ERROR("DetectDoorAction: failed to detect a door");
return false;
}
- cout << "end door detection action " << res_doordetect.doors[0] << endl;
door_out = res_doordetect.doors[0];
return true;
}
-
-
-void DetectDoorAction::handlePreempt()
-{
- request_preempt_ = true;
-};
-
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -64,59 +64,51 @@
-void DetectHandleAction::handleActivate(const robot_msgs::Door& door)
+robot_actions::ResultStatus DetectHandleAction::execute(const robot_msgs::Door& door, robot_msgs::Door& result)
{
- ROS_INFO("DetectHandleAction: handle activate");
- request_preempt_ = false;
- notifyActivated();
+ ROS_INFO("DetectHandleAction: execute");
cout << "begin handle detection action " << door << endl;
-
// laser detection
- robot_msgs::Door result_laser, result_camera, result;
+ robot_msgs::Door result_laser, result_camera;
if (!laserDetection(door, result_laser)){
- if (request_preempt_){
+ if (isPreemptRequested()){
ROS_INFO("DetectHandleAction: Preempted");
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
else{
ROS_ERROR("DetectHandleAction: Aborted laser detection");
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
}
if (!door_handle_detector::transformTo(tf_, fixed_frame, result_laser, result_laser)){
ROS_ERROR ("DetectHandleAction: Could not transform laser door message from frame %s to frame %s.",
result_laser.header.frame_id.c_str (), fixed_frame.c_str ());
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
cout << "result laser " << result_laser << endl;
ROS_INFO("DetectHandleAction: detected handle position transformed to '%s'", fixed_frame.c_str());
-
// camera detection
if (!cameraDetection(door, result_camera)){
- if (request_preempt_){
+ if (isPreemptRequested()){
ROS_INFO("DetectHandleAction: Preempted");
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
else{
ROS_ERROR("DetectHandleAction: Aborted camera detection");
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
}
+
if (!door_handle_detector::transformTo(tf_, fixed_frame, result_camera, result_camera)){
ROS_ERROR ("DetectHandleAction: Could not transform camera door message from frame %s to frame %s.",
result_camera.header.frame_id.c_str (), fixed_frame.c_str ());
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
+
cout << "result camera " << result_camera << endl;
ROS_INFO("DetectHandleAction: detected handle position transformed to '%s'", fixed_frame.c_str());
@@ -130,8 +122,7 @@
if (error > 0.1){
ROS_ERROR("DetectHandleAction: Aborted because error between laser and camera result is too big");
- notifyAborted(door);
- return;
+ return robot_actions::ABORTED;
}
// store handle position
@@ -149,22 +140,22 @@
cout << "result laser+camera " << result << endl;
ROS_INFO("DetectHandleAction: Succeeded");
- notifySucceeded(result);
+ return robot_actions::SUCCESS;
}
-
bool DetectHandleAction::laserDetection(const robot_msgs::Door& door_in,
robot_msgs::Door& door_out)
{
// check where robot is relative to the door
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
if (!tf_.canTransform("base_footprint", "laser_tilt_link", ros::Time(), ros::Duration().fromSec(5.0))){
ROS_ERROR("DetectHandleAction: could not get transform from 'base_footprint' to 'laser_tilt_link'");
return false;
}
tf::Stamped<tf::Transform> tilt_stage;
tf_.lookupTransform("base_footprint", "laser_tilt_link", ros::Time(), tilt_stage);
+ ROS_INFO("DetectHandleAction: handle activate");
double laser_height = tilt_stage.getOrigin()[2];
tf::Stamped<tf::Vector3> handlepoint(tf::Vector3((door_in.door_p1.x+door_in.door_p2.x)/2.0,
(door_in.door_p1.y+door_in.door_p2.y)/2.0,
@@ -181,7 +172,7 @@
ROS_INFO("DetectHandleAction: tilt laser is at height %f, and door at distance %f", laser_height, dist);
// gets a point cloud from the point_cloud_srv
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
ROS_INFO("DetectHandleAction: get a point cloud from the door");
point_cloud_assembler::BuildCloudAngle::Request req_pointcloud;
point_cloud_assembler::BuildCloudAngle::Response res_pointcloud;
@@ -194,7 +185,7 @@
}
// detect handle
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
ROS_INFO("DetectHandleAction: detect the handle");
door_handle_detector::DoorsDetectorCloud::Request req_handledetect;
door_handle_detector::DoorsDetectorCloud::Response res_handledetect;
@@ -209,30 +200,26 @@
return true;
}
-
-
-
bool DetectHandleAction::cameraDetection(const robot_msgs::Door& door_in,
robot_msgs::Door& door_out)
{
// make the head point towards the door
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
ROS_INFO("DetectHandleAction: point head towards door");
robot_msgs::PointStamped door_pnt;
door_pnt.header.frame_id = door_in.header.frame_id;
door_pnt.point.x = (door_in.door_p1.x+door_in.door_p2.x)/2.0;
door_pnt.point.y = (door_in.door_p1.y+door_in.door_p2.y)/2.0;
door_pnt.point.z = 0.9;
- cout << "head point: ("
+ cout << "door_pnt.point " << door_in.header.frame_id << " "
<< door_pnt.point.x << " "
- << door_pnt.point.y << " "
- << door_pnt.point.z << ")" << endl;
-
+ << door_pnt.point.y << " "
+ << door_pnt.point.z << endl;
node_.publish("head_controller/head_track_point", door_pnt);
ros::Duration().fromSec(2).sleep();
// detect handle
- if (request_preempt_) return false;
+ if (isPreemptRequested()) return false;
ROS_INFO("DetectHandleAction: detect the handle");
door_handle_detector::DoorsDetector::Request req_handledetect;
door_handle_detector::DoorsDetector::Response res_handledetect;
@@ -245,11 +232,3 @@
door_out = res_handledetect.doors[0];
return true;
}
-
-
-
-void DetectHandleAction::handlePreempt()
-{
- request_preempt_ = true;
-};
-
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_grasp_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_grasp_door.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_grasp_door.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -49,7 +49,6 @@
GraspDoorAction::GraspDoorAction(Node& node) :
robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("grasp_handle"),
node_(node),
- request_preempt_(false),
tf_(node)
{
node_.advertise<std_msgs::Float64>("gripper_effort/set_command",10);
@@ -61,82 +60,84 @@
-void GraspDoorAction::handleActivate(const robot_msgs::Door& door)
+robot_actions::ResultStatus GraspDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
{
- notifyActivated();
-
+ feedback = goal;
+
// door needs to be in time fixed frame
- if (door.header.frame_id != fixed_frame)
- notifyAborted(door);
- Vector normal(door.normal.x, door.normal.y, door.normal.z);
- Vector handle(door.handle.x, door.handle.y, door.handle.z);
+ if (goal.header.frame_id != fixed_frame){
+ return robot_actions::ABORTED;
+ }
+
+ Vector normal(goal.normal.x, goal.normal.y, goal.normal.z);
+ Vector handle(goal.handle.x, goal.handle.y, goal.handle.z);
Stamped<Pose> gripper_pose;
gripper_pose.frame_id_ = fixed_frame;
// open the gripper
- if (request_preempt_) {notifyPreempted(door); return;}
+ if (isPreemptRequested()) {
+ return robot_actions::PREEMPTED;
+ }
+
std_msgs::Float64 gripper_msg;
gripper_msg.data = 2.0;
node_.publish("gripper_effort/set_command", gripper_msg);
// move gripper in front of door
- if (request_preempt_) {notifyPreempted(door); return;}
+ if (isPreemptRequested()) {
+ return robot_actions::PREEMPTED;
+ }
+
gripper_pose.setOrigin( Vector3(handle(0) + (normal(0) * -0.15), handle(1) + (normal(1) * -0.15), handle(2) + (normal(2) * -0.15)));
- gripper_pose.setRotation( Quaternion(getDoorAngle(door), 0, M_PI/2.0) );
+ gripper_pose.setRotation( Quaternion(getDoorAngle(goal), 0, M_PI/2.0) );
gripper_pose.stamp_ = Time::now();
PoseStampedTFToMsg(gripper_pose, req_moveto.pose);
+
if (!ros::service::call("cartesian_trajectory_right/move_to", req_moveto, res_moveto)){
- if (request_preempt_)
- notifyPreempted(door);
- else
- notifyAborted(door);
- return;
+ if (isPreemptRequested())
+ return robot_actions::PREEMPTED;
+
+ return robot_actions::ABORTED;
}
// move gripper over door handle
- if (request_preempt_) {notifyPreempted(door); return;}
+ if (isPreemptRequested()) {
+ return robot_actions::PREEMPTED;
+ }
+
gripper_pose.frame_id_ = fixed_frame;
gripper_pose.setOrigin( Vector3(handle(0) + (normal(0) * 0.2 ), handle(1) + (normal(1) * 0.2), handle(2) + (normal(2) * 0.2)));
- gripper_pose.setRotation( Quaternion(getDoorAngle(door), 0, M_PI/2.0) );
+ gripper_pose.setRotation( Quaternion(getDoorAngle(goal), 0, M_PI/2.0) );
gripper_pose.stamp_ = Time::now();
PoseStampedTFToMsg(gripper_pose, req_moveto.pose);
+
if (!ros::service::call("cartesian_trajectory_right/move_to", req_moveto, res_moveto)){
- if (request_preempt_)
- notifyPreempted(door);
- else
- notifyAborted(door);
- return;
+ if (isPreemptRequested())
+ return robot_actions::PREEMPTED;
+
+ return robot_actions::ABORTED;
}
// close the gripper
- if (request_preempt_) {notifyPreempted(door); return;}
+ if (isPreemptRequested()) {
+ return robot_actions::PREEMPTED;
+ }
+
gripper_msg.data = -2.0;
node_.publish("gripper_effort/set_command", gripper_msg);
for (unsigned int i=0; i<100; i++){
Duration().fromSec(4.0/100.0).sleep();
- if (request_preempt_) {
+ if (isPreemptRequested()) {
gripper_msg.data = 0.0;
node_.publish("gripper_effort/set_command", gripper_msg);
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
}
- notifySucceeded(door);
+ return robot_actions::SUCCESS;
}
-
-
-void GraspDoorAction::handlePreempt()
-{
- request_preempt_ = true;
- ros::service::call("cartesian_trajectory_right/preempt", req_empty, res_empty);
-};
-
-
-
-
double GraspDoorAction::getDoorAngle(const robot_msgs::Door& door)
{
Vector normal(door.normal.x, door.normal.y, door.normal.z);
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -48,8 +48,7 @@
OpenDoorAction::OpenDoorAction() :
robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("open_door"),
- node_(ros::Node::instance()),
- request_preempt_(false)
+ node_(ros::Node::instance())
{};
@@ -59,10 +58,10 @@
-void OpenDoorAction::handleActivate(const robot_msgs::Door& door)
-{
- notifyActivated();
-
+robot_actions::ResultStatus OpenDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+{
+ feedback = goal;
+
// stop
tff_stop_.mode.vel.x = tff_stop_.FORCE;
tff_stop_.mode.vel.y = tff_stop_.FORCE;
@@ -108,17 +107,14 @@
tff_door_.value.rot.y = 0.0;
tff_door_.value.rot.z = 0.0;
-
-
// turn handle
for (unsigned int i=0; i<100; i++){
tff_handle_.value.rot.x += -1.5/100.0;
node_->publish("cartesian_tff_right/command", tff_handle_);
Duration().fromSec(4.0/100.0).sleep();
- if (request_preempt_) {
+ if (isPreemptRequested()) {
node_->publish("cartesian_tff_right/command", tff_stop_);
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
}
@@ -126,23 +122,13 @@
node_->publish("cartesian_tff_right/command", tff_door_);
for (unsigned int i=0; i<500; i++){
Duration().fromSec(10.0/500.0).sleep();
- if (request_preempt_) {
+ if (isPreemptRequested()) {
node_->publish("cartesian_tff_right/command", tff_stop_);
- notifyPreempted(door);
- return;
+ return robot_actions::PREEMPTED;
}
}
// finish
node_->publish("cartesian_tff_right/command", tff_stop_);
-
- notifySucceeded(door);
+ return robot_actions::SUCCESS;
}
-
-
-
-
-void OpenDoorAction::handlePreempt()
-{
- request_preempt_ = true;
-};
Modified: pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -74,8 +74,9 @@
runner.connect<robot_msgs::Door, door_handle_detector::DetectDoorActionStatus, robot_msgs::Door>(handle_detector);
runner.run();
- door_detector.handleActivate(my_door_);
- handle_detector.handleActivate(door_detector.tmp_result_);
+ robot_msgs::Door feedback;
+ door_detector.execute(my_door_, feedback);
+ handle_detector.execute(door_detector.tmp_result_, feedback);
return (0);
}
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ros_action_adapter.h
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ros_action_adapter.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ros_action_adapter.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -6,6 +6,7 @@
#include "TokenVariable.hh"
#include "SymbolDomain.hh"
#include "StringDomain.hh"
+#include <std_msgs/Empty.h>
namespace TREX {
@@ -73,7 +74,7 @@
ROS_INFO("%sRegistering publisher for %s on topic: %s",
nameString().c_str(), timelineName.c_str(), _preempt_topic.c_str());
- m_node->registerPublisher<Goal>(_preempt_topic, QUEUE_MAX());
+ m_node->registerPublisher<std_msgs::Empty>(_preempt_topic, QUEUE_MAX());
}
Observation* getObservation(){
@@ -175,7 +176,8 @@
m_node->publishMsg<Goal>(_request_topic, goal_msg);
}
else {
- m_node->publishMsg<Goal>(_preempt_topic, goal_msg);
+ std_msgs::Empty recall_msg;
+ m_node->publishMsg<std_msgs::Empty>(_preempt_topic, recall_msg);
}
return true;
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc 2009-04-14 14:11:06 UTC (rev 13808)
@@ -39,6 +39,7 @@
* @author Conor McGann
*/
#include <executive_trex_pr2/adapters.h>
+#include <robot_actions/action.h>
#include <boost/thread.hpp>
#include <cstdlib>
@@ -48,29 +49,31 @@
*/
namespace executive_trex_pr2 {
- template <class T>
- class StubAction: public robot_actions::Action<T, T> {
+
+
+ template <class Goal, class Feedback>
+ class StubAction: public robot_actions::Action<Goal, Feedback> {
public:
- StubAction(const std::string& name): robot_actions::Action<T, T>(name) {}
+ StubAction(const std::string& name): robot_actions::Action<Goal, Feedback>(name) {}
protected:
- // Activation does all the real work
- virtual void handleActivate(const T& msg){
- // Immediate reply
- robot_actions::Action<T, T>::notifyActivated();
- _state = msg;
- notifySucceeded(msg);
+ virtual robot_actions::ResultStatus execute(const Goal& goal, Feedback& feedback){
+ ROS_DEBUG("Executing %s\n", robot_actions::Action<Goal, Feedback>::getName().c_str());
+ return robot_actions::SUCCESS;
}
+ };
- // Activation does all the real work
- virtual void handlePreempt(){
- // Immediate reply
- notifyPreempted(_state);
- }
+ template <class T> class SimpleStubAction: public robot_actions::Action<T,T> {
+ public:
+ SimpleStubAction(const std::string& name): robot_actions::Action<T, T>(name) {}
- T _state;
+ virtual robot_actions::ResultStatus execute(const T& goal, T& feedback){
+ feedback = goal;
+ ROS_DEBUG("Executing %s\n", robot_actions::Action<T, T>::getName().c_str());
+ return robot_actions::SUCCESS;
+ }
};
/**
@@ -113,31 +116,6 @@
boost::thread* _update_thread;
};
-
- template <class Goal, class Feedback>
- class StubAction1: public robot_actions::Action<Goal, Feedback> {
- public:
-
- StubAction1(const std::string& name): robot_actions::Action<Goal, Feedback>(name) {}
-
- protected:
-
- // Activation does all the real work
- virtual void handleActivate(const Goal& msg){
- // Immediate reply
- robot_actions::Action<Goal, Feedback>::notifyActivated();
- notifySucceeded(_feedback);
- }
-
- // Activation does all the real work
- virtual void handlePreempt(){
- // Immediate reply
- notifyPreempted(_feedback);
- }
-
- Feedback _feedback;
- };
-
}
@@ -177,105 +155,105 @@
/* Add action stubs for doors */
// Detect Door
- executive_trex_pr2::StubAction<robot_msgs::Door> detect_door("detect_door");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> detect_door("detect_door");
if (getComponentParam("/trex/enable_detect_door"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(detect_door);
// Detect Handle
- executive_trex_pr2::StubAction<robot_msgs::Door> detect_handle("detect_handle");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> detect_handle("detect_handle");
if (getComponentParam("/trex/enable_detect_handle"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(detect_handle);
// Grasp Handle
- executive_trex_pr2::StubAction<robot_msgs::Door> grasp_handle("grasp_handle");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> grasp_handle("grasp_handle");
if (getComponentParam("/trex/enable_grasp_handle"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(grasp_handle);
- executive_trex_pr2::StubAction<robot_msgs::Door> open_door("open_door");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> open_door("open_door");
if (getComponentParam("/trex/enable_open_door"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(open_door);
- executive_trex_pr2::StubAction<robot_msgs::Door> open_door_without_grasp("open_door_without_grasp");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> open_door_without_grasp("open_door_without_grasp");
if (getComponentParam("/trex/enable_open_door_without_grasp"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(open_door_without_grasp);
- executive_trex_pr2::StubAction<robot_msgs::Door> release_door("release_door");
+ executive_trex_pr2::SimpleStubAction<robot_msgs::Door> release_door("release_door");
if (getComponentParam("/trex/enable_release_door"))
runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(release_door);
/* Action stubs for plugs */
- executive_trex_pr2::StubAction1<std_msgs::Empty, robot_msgs::PlugStow> detect_plug_on_base("detect_plug_on_base");
+ executive_trex_pr2::StubAction<std_msgs::Empty, robot_msgs::PlugStow> detect_plug_on_base("detect_plug_on_base");
if (getComponentParam("/trex/enable_detect_plug_on_base"))
runner.connect<std_msgs::Empty, robot_actions::DetectPlugOnBaseState, robot_msgs::PlugStow>(detect_plug_on_base);
- executive_trex_pr2::StubAction1<robot_msgs::PlugStow, std_msgs::Empty> move_and_grasp_plug("move_and_grasp_plug");
+ executive_trex_pr2::StubAction<robot_msgs::PlugStow, std_msgs::Empty> move_and_grasp_plug("move_and_grasp_plug");
if (getComponentParam("/trex/enable_move_and_grasp_plug"))
runner.connect<robot_msgs::PlugStow, robot_actions::MoveAndGraspPlugState, std_msgs::Empty>(move_and_grasp_plug);
- executive_trex_pr2::StubAction1<robot_msgs::PlugStow, std_msgs::Empty> stow_plug("stow_plug");
+ executive_trex_pr2::StubAction<robot_msgs::PlugStow, std_msgs::Empty> stow_plug("stow_plug");
if (getComponentParam("/trex/enable_stow_plug"))
runner.connect<robot_msgs::PlugStow, robot_actions::StowPlugState, std_msgs::Empty>(stow_plug);
- executive_trex_pr2::StubAction<std_msgs::Empty> plugs_untuck_arms("plugs_untuck_arms");
+ executive_trex_pr2::SimpleStubAction<std_msgs::Empty> plugs_untuck_arms("plugs_untuck_arms");
if (getComponentParam("/trex/enable_plugs_untuck_arms"))
runner.connect<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty>(plugs_untuck_arms);
- executive_trex_pr2::StubAction<std_msgs::Empty> localize_plug_in_gripper("localize_plug_in_gripper");
+ executive_trex_pr2::SimpleStubAction<std_msgs::Empty> localize_plug_in_gripper("localize_plug_in_gripper");
if (getComponentParam("/trex/enable_localize_plug_in_gripper"))
runner.connect<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty>(localize_plug_in_gripper);
- executive_trex_pr2::StubAction<std_msgs::Empty> unplug("unplug");
+ executive_trex_pr2::SimpleStubAction<std_msgs::Empty> unplug("unplug");
if (getComponentParam("/trex/enable_unplug"))
runner.connect<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty>(unplug);
- executive_trex_pr2::StubAction<std_msgs::Empty> push_plug_in("push_plug_in");
+ executive_trex_pr2::SimpleStubAction<std_msgs::Empty> push_plug_in("push_plug_in");
if (getComponentParam("/trex/enable_push_plug_in"))
runner.connect<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty>(push_plug_in);
- executive_trex_pr2::StubAction<std_msgs::Empty> insert_plug("insert_plug");
+ executive_trex_pr2::SimpleStubAction<std_msgs::Empty> insert_plug("insert_plug");
if (getComponentParam("/trex/enable_insert_plug"))
runner.connect<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty>(insert_plug);
- executive_trex_pr2::StubAction1<robot_actions::ServoToOutlet, std_msgs::Empty> servo_to_outlet("servo_to_outlet");
+ executive_trex_pr2::StubAction<robot_actions::ServoToOutlet, std_msgs::Empty> servo_to_outlet("servo_to_outlet");
if (getComponentParam("/trex/enable_servo_to_outlet"))
runner.connect<robot_actions::ServoToOutlet, robot_actions::ServoToOutletState, std_msgs::Empty>(servo_to_outlet);
- executive_trex_pr2::StubAction1<robot_msgs::PointStamped, robot_msgs::PoseStamped> detect_outlet_fine("detect_outlet_fine");
+ executive_trex_pr2::StubAction<robot_msgs::PointStamped, robot_msgs::PoseStamped> detect_outlet_fine("detect_outlet_fine");
if (getComponentParam("/trex/enable_detect_outlet_fine"))
runner.connect<robot_msgs::PointStamped, robot_actions::DetectOutletState, robot_msgs::PoseStamped>(detect_outlet_fine);
- executive_trex_pr2::StubAction1<robot_msgs::PointStamped, robot_msgs::PoseStamped> detect_outlet_coarse("detect_outlet_coarse");
+ executive_trex_pr2::StubAction<robot_msgs::PointStamped, robot_msgs::PoseStamped> detect_outlet_coarse("detect_outlet_coarse");
if (getComponentParam("/trex/enable_detect_outlet_coarse"))
runner.connect<robot_msgs::PointStamped, robot_actions::DetectOutletState, robot_msgs::PoseStamped>(detect_outlet_coarse);
/* Action stubs for resource management */
- executive_trex_pr2::StubAction1<robot_actions::SwitchControllers, std_msgs::Empty> switch_controllers("switch_controllers");
+ executive_trex_pr2::StubAction<robot_actions::SwitchControllers, std_msgs::Empty> switch_controllers("switch_controllers");
if (getComponentParam("/trex/enable_switch_controllers"))
runner.connect<robot_actions::SwitchControllers, robot_actions::SwitchControllersState, std_msgs::Empty>(switch_controllers);
// Allocate other action stubs
- executive_trex_pr2::StubAction<robot_actions::Pose2D> move_base("move_base");
+ executive_trex_pr2::SimpleStubAction<robot_actions::Pose2D> move_base("move_base");
if (getComponentParam("/trex/enable_move_base"))
runner.connect<robot_actions::Pose2D, robot_actions::MoveBaseState, robot_actions::Pose2D>(move_base);
- executive_trex_pr2::StubAction<std_msgs::Float32> recharge("recharge_controller");
+ executive_trex_pr2::SimpleStubAction<std_msgs::Float32> recharge("recharge_controller");
if (getComponentParam("/trex/enable_recharge"))
runner.connect<std_msgs::Float32, robot_actions::RechargeState, std_msgs::Float32>(recharge);
- executive_trex_pr2::StubAction<std_msgs::String> shell_command("shell_command");
+ executive_trex_pr2::SimpleStubAction<std_msgs::String> shell_command("shell_command");
if (getComponentParam("/trex/enable_shell_command"))
runner.connect<std_msgs::String, robot_actions::ShellCommandState, std_msgs::String>(shell_command);
- executive_trex_pr2::StubAction<robot_actions::Pose2D> check_doorway("check_doorway");
+ executive_trex_pr2::SimpleStubAction<robot_actions::Pose2D> check_doorway("check_doorway");
if (getComponentParam("/trex/enable_check_doorway"))
runner.connect<robot_actions::Pose2D, robot_actions::CheckDoorwayState, robot_actions::Pose2D>(check_doorway);
- executive_trex_pr2::StubAction<robot_actions::Pose2D> notify_door_blocked("notify_door_blocked");
+ executive_trex_pr2::SimpleStubAction<robot_actions::Pose2D> notify_door_blocked("notify_door_blocked");
if (getComponentParam("/trex/enable_notify_door_blocked"))
runner.connect<robot_actions::Pose2D, robot_actions::NotifyDoorBlockedState, robot_actions::Pose2D>(notify_door_blocked);
- executive_trex_pr2::StubAction<std_msgs::Empty> safety_tuck_arms("safety_tuck_arms");
+ executive_trex_pr2::SimpleStubAction<std_msgs::Empty> safety_tuck_arms("safety_tuck_arms");
if (getComponentParam("/trex/enable_safety_tuck_arms"))
runner.connect<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty>(safety_tuck_arms);
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/doors/stubs.launch
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/doors/stubs.launch 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/doors/stubs.launch 2009-04-14 14:11:06 UTC (rev 13808)
@@ -2,6 +2,7 @@
<master auto="start"/>
<param name="/trex/enable_base_state_publisher" value="true"/>
<param name="/trex/enable_switch_controllers" value="true"/>
+ <param name="/trex/enable_safety_tuck_arms" value="true"/>
<param name="/trex/enable_detect_door" value="true"/>
<param name="/trex/enable_detect_handle" value="true"/>
<param name="/trex/enable_grasp_handle" value="true"/>
Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_fine_outlet_detect.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_fine_outlet_detect.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_fine_outlet_detect.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -62,8 +62,7 @@
public:
FineOutletDetectAction();
- virtual void handleActivate(const robot_msgs::PointStamped& point);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::PointStamped& point, robot_msgs::PoseStamped& feedback);
private:
ros::Node* node_;
Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_move_and_grasp_plug.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_move_and_grasp_plug.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_move_and_grasp_plug.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -62,10 +62,8 @@
MoveAndGraspPlugAction();
~MoveAndGraspPlugAction();
- virtual void handleActivate(const robot_msgs::PlugStow& plug_stow);
- virtual void handlePreempt();
+ robot_actions::ResultStatus execute(const robot_msgs::PlugStow& plug_stow, std_msgs::Empty&);
-
private:
void reset();
Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_stow_plug.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_stow_plug.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_stow_plug.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -62,10 +62,8 @@
StowPlugAction();
~StowPlugAction();
- virtual void handleActivate(const robot_msgs::PlugStow& plug_stow);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const robot_msgs::PlugStow& plug_stow, std_msgs::Empty&);
-
private:
void reset();
Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_untuck_arms.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_untuck_arms.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_untuck_arms.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -57,11 +57,8 @@
UntuckArmsAction();
~UntuckArmsAction();
- virtual void handleActivate(const std_msgs::Empty& empty);
- virtual void handlePreempt();
+ virtual robot_actions::ResultStatus execute(const std_msgs::Empty& empty, std_msgs::Empty& feedback);
-
-
private:
// average the last couple plug centroids
bool isTrajectoryDone();
Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_fine_outlet_detect.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_fine_outlet_detect.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_fine_outlet_detect.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -43,19 +43,12 @@
{
}
-void FineOutletDetectAction::handleActivate(const robot_msgs::PointStamped& point)
-{
- notifyActivated();
+robot_actions::ResultStatus FineOutletDetectAction::execute(const robot_msgs::PointStamped& point, robot_msgs::PoseStamped& feedback){
req_.point = point;
bool success = ros::service::call("/outlet_detector/fine_outlet_detect", req_, res_);
if (success)
- notifySucceeded(res_.pose);
+ return robot_actions::SUCCESS;
else
- notifyAborted(res_.pose);
+ return robot_actions::ABORTED;
}
-
-void FineOutletDetectAction::handlePreempt()
-{
- // TODO: allow preemption?
-}
Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_move_and_grasp_plug.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_move_and_grasp_plug.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_move_and_grasp_plug.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -53,12 +53,14 @@
if(gripper_controller_ == "" )
{
ROS_ERROR("%s: Aborted, gripper controller param was not set.", action_name_.c_str());
+ terminate();
return;
}
if(arm_controller_ == "" )
{
ROS_ERROR("%s: Aborted, arm controller param was not set.", action_name_.c_str());
+ terminate();
return;
}
@@ -68,10 +70,8 @@
{
};
-void MoveAndGraspPlugAction::handleActivate(const robot_msgs::PlugStow& plug_stow)
+robot_actions::ResultStatus MoveAndGraspPlugAction::execute(const robot_msgs::PlugStow& plug_stow, std_msgs::Empty& feedback)
{
- notifyActivated();
-
plug_stow_ = plug_stow;
reset();
@@ -79,19 +79,11 @@
node_->publish(gripper_controller_ + "/set_command", gripper_cmd_);
moveToGrasp();
graspPlug();
- return;
+ return waitForDeactivation(feedback);
}
-void MoveAndGraspPlugAction::handlePreempt()
-{
- ROS_INFO("%s: Preempted.", action_name_.c_str());
- notifyPreempted(empty_);
- return;
-}
-
void MoveAndGraspPlugAction::reset()
{
-
last_grasp_value_ = 10.0;
grasp_count_ = 0;
gripper_cmd_.data = 0.04;
@@ -104,27 +96,25 @@
req_pose_.pose.pose.orientation.y = 0.13;
req_pose_.pose.pose.orientation.z = 0.68;
req_pose_.pose.pose.orientation.w = 0.68;
-
-
}
void MoveAndGraspPlugAction::moveToGrasp()
{
- if (!isActive())
- return;
-
node_->publish(gripper_controller_ + "/set_command", gripper_cmd_);
req_pose_.pose.header.stamp = ros::Time();
if (!ros::service::call(arm_controller_ + "/move_to", req_pose_, res_pose_))
{
ROS_ERROR("%s: Failed to move arm.", action_name_.c_str());
- notifyAborted(empty_);
+ deactivate(robot_actions::ABORTED, empty_);
return;
}
- if (!isActive())
+ if (isPreemptRequested()){
+ deactivate(robot_actions::PREEMPTED, empty_);
return;
+ }
+
node_->publish(gripper_controller_ + "/set_command", gripper_cmd_);
req_pose_.pose.pose.position.z = plug_stow_.plug_centroid.z - 0.1;
@@ -132,9 +122,10 @@
if (!ros::service::call(arm_controller_ + "/move_to", req_pose_, res_pose_))
{
ROS_ERROR("%s: Failed to move arm.", action_name_.c_str());
- notifyAborted(empty_);
+ deactivate(robot_actions::ABORTED, empty_);
return;
}
+
gripper_cmd_.data = 0.0;
return;
}
@@ -142,12 +133,9 @@
void MoveAndGraspPlugAction::graspPlug()
{
- if (!isActive())
- return;
-
node_->publish(gripper_controller_ + "/set_command", gripper_cmd_);
node_->subscribe(gripper_controller_ + "/state", controller_state_msg_, &MoveAndGraspPlugAction::checkGrasp, this, 1);
-
+ deactivate(robot_actions::SUCCESS, empty_);
return;
}
@@ -176,18 +164,16 @@
{
ROS_INFO("%s: Error, failed to grasp plug.", action_name_.c_str());
node_->unsubscribe(gripper_controller_ + "/state");
- notifyAborted(empty_);
+ deactivate(robot_actions::ABORTED, empty_);
return;
}
else
{
ROS_INFO("%s: succeeded.", action_name_.c_str());
node_->unsubscribe(gripper_controller_ + "/state");
- notifySucceeded(empty_);
+ deactivate(robot_actions::SUCCESS, empty_);
return;
}
}
-
- return;
}
Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_stow_plug.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_stow_plug.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_stow_plug.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -68,26 +68,17 @@
{
};
-void StowPlugAction::handleActivate(const robot_msgs::PlugStow& plug_stow)
+robot_actions::ResultStatus StowPlugAction::execute(const robot_msgs::PlugStow& plug_stow, std_msgs::Empty& feedback)
{
- notifyActivated();
-
plug_stow_ = plug_stow;
reset();
moveToStow();
releasePlug();
- return;
+ return waitForDeactivation(feedback);
}
-void StowPlugAction::handlePreempt()
-{
- ROS_INFO("%s: Preempted.", action_name_.c_str());
- notifyPreempted(empty_);
- return;
-}
-
void StowPlugAction::reset()
{
@@ -108,23 +99,19 @@
}
-
void StowPlugAction::moveToStow()
-{
- if (!isActive())
- return;
-
-
+{
req_pose_.pose.header.stamp = ros::Time();
if (!ros::service::call(arm_controller_ + "/move_to", req_pose_, res_pose_))
{
ROS_ERROR("%s: Failed to move arm.", action_name_.c_str());
- notifyAborted(empty_);
+ deactivate(robot_actions::ABORTED, empty_);
return;
}
- if (!isActive())
+ if (isPreemptRequested())
return;
+
node_->publish(gripper_controller_ + "/set_command", gripper_cmd_);
req_pose_.pose.pose.position.z = plug_stow_.plug_centroid.z - 0.1;
@@ -132,28 +119,34 @@
if (!ros::service::call(arm_controller_ + "/move_to", req_pose_, res_pose_))
{
ROS_ERROR("%s: Failed to move arm.", action_name_.c_str());
- notifyAborted(empty_);
- return;
+ deactivate(robot_actions::ABORTED, empty_);
}
+
return;
}
void StowPlugAction::releasePlug()
{
- if (!isActive())
+
+ if (isPreemptRequested())
return;
node_->publish(gripper_controller_ + "/set_command", gripper_cmd_);
+
+ // DO U REALLY WANT TO SUBSCRIBE EACH TIME? WHAT HAPPENS IF U DO NOT?
node_->subscribe(gripper_controller_ + "/state", controller_state_msg_, &StowPlugAction::checkGrasp, this, 1);
-
- return;
}
void StowPlugAction::checkGrasp()
{
if (!isActive())
return;
+
+ if (isPreemptRequested()){
+ deactivate(robot_actions::PREEMPTED, std_msgs::Empty());
+ return;
+ }
node_->publish(gripper_controller_ + "/set_command", gripper_cmd_);
@@ -175,14 +168,14 @@
{
ROS_INFO("%s: Error, failed to release plug.", action_name_.c_str());
node_->unsubscribe(gripper_controller_ + "/state");
- notifyAborted(empty_);
+ deactivate(robot_actions::ABORTED, std_msgs::Empty());
return;
}
else
{
ROS_INFO("%s: succeeded.", action_name_.c_str());
node_->unsubscribe(gripper_controller_ + "/state");
- notifySucceeded(empty_);
+ deactivate(robot_actions::SUCCESS, std_msgs::Empty());
return;
}
}
Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_untuck_arms.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_untuck_arms.cpp 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_untuck_arms.cpp 2009-04-14 14:11:06 UTC (rev 13808)
@@ -51,6 +51,7 @@
if(which_arms_ == "")
{
ROS_ERROR("%s: Aborted, which arms param was not set.", action_name_.c_str());
+ terminate();
return;
}
@@ -62,6 +63,7 @@
if(right_arm_controller_ == "")
{
ROS_ERROR("%s: Aborted, right arm controller param was not set.", action_name_.c_str());
+ terminate();
return;
}
}
@@ -73,6 +75,7 @@
if(left_arm_controller_ == "")
{
ROS_ERROR("%s: Aborted, left arm controller param was not set.", action_name_.c_str());
+ terminate();
return;
}
}
@@ -111,69 +114,60 @@
{
};
-void UntuckArmsAction::handleActivate(const std_msgs::Empty& empty)
-{
- notifyActivated();
-
- if((which_arms_ == "both") || (which_arms_ == "right"))
- {
- if(!ros::service::call(right_arm_controller_ + "/TrajectoryStart", right_traj_req_, traj_res_))
+robot_actions::ResultStatus UntuckArmsAction::execute(const std_msgs::Empty& empty, std_msgs::Empty& feedback)
+{
+ if(!ros::service::call(right_arm_controller_ + "/TrajectoryStart", right_traj_req_, traj_res_))
{
ROS_ERROR("%s: Aborted, failed to start right arm trajectory.", action_name_.c_str());
- notifyAborted(empty_);
- return;
- }
- traj_id_ = traj_res_.trajectoryid;
- current_controller_name_ = right_arm_controller_;
+ return robot_actions::ABORTED;
+ }
+
+ traj_id_ = traj_res_.trajectoryid;
+ current_controller_name_ = right_arm_controller_;
- while(!isTrajectoryDone() && !traj_error_)
+ while(!isTrajectoryDone() && !traj_error_)
{
- if (!isActive())
- return;
+ if (isPreemptRequested()){
+ cancelTrajectory();
+ ROS_INFO("%s: Preempted.", action_name_.c_str());
+ return robot_actions::PREEMPTED;
+ }
+
sleep(0.5);
}
- }
if((which_arms_ == "both") || (which_arms_ == "left") && !traj_error_)
- {
- if(!ros::service::call(left_arm_controller_ + "/TrajectoryStart", left_traj_req_, traj_res_))
{
- ROS_ERROR("%s: Aborted, failed to start left arm trajectory.", action_name_.c_str());
- notifyAborted(empty_);
- return;
- }
- traj_id_ = traj_res_.trajectoryid;
- current_controller_name_ = left_arm_controller_;
+ if(!ros::service::call(left_arm_controller_ + "/TrajectoryStart", left_traj_req_, traj_res_))
+ {
+ ROS_ERROR("%s: Aborted, failed to start left arm trajectory.", action_name_.c_str());
+ return robot_actions::ABORTED;
+ }
+ traj_id_ = traj_res_.trajectoryid;
+ current_controller_name_ = left_arm_controller_;
- while(!isTrajectoryDone() && !traj_error_)
- {
- if (!isActive())
- return;
- sleep(0.5);
+ while(!isTrajectoryDone() && !traj_error_)
+ {
+ if (isPreemptRequested()){
+ cancelTrajectory();
+ ROS_INFO("%s: Preempted.", action_name_.c_str());
+ return robot_actions::PREEMPTED;
+ }
+
+ sleep(0.5);
+ }
}
- }
if(traj_error_)
- {
- ROS_ERROR("%s: Aborted, trajectory controller failed to reach goal.", action_name_.c_str());
- notifyAborted(empty_);
- return;
- }
+ {
+ ROS_ERROR("%s: Aborted, trajectory controller failed to reach goal.", action_name_.c_str());
+ return robot_actions::ABORTED;
+ }
- notifySucceeded(empty_);
- return;
+ return robot_actions::SUCCESS;
}
-void UntuckArmsAction::handlePreempt()
-{
- cancelTrajectory();
- ROS_INFO("%s: Preempted.", action_name_.c_str());
- notifyPreempted(empty_);
- return;
-}
-
-
bool UntuckArmsAction::isTrajectoryDone()
{
bool done = false;
Modified: pkg/trunk/highlevel/robot_actions/include/robot_actions/action.h
===================================================================
--- pkg/trunk/highlevel/robot_actions/include/robot_actions/action.h 2009-04-14 11:58:37 UTC (rev 13807)
+++ pkg/trunk/highlevel/robot_actions/include/robot_actions/action.h 2009-04-14 14:11:06 UTC (rev 13808)
@@ -48,10 +48,21 @@
#include <robot_actions/ActionStatus.h>
#include <boost/bind.hpp>
+#include <boost/thread.hpp>
namespace robot_actions {
/**
+ * @brief Result codes for execution of an action. All values are sub states of the inactive state for an action.
+ * @see ActionStatus
+ */
+ enum ResultStatus {
+ SUCCESS = 1,
+ ABORTED = 2,
+ PREEMPTED = 3
+ };
+
+ /**
* @brief Abstract base class for a durative, preemptable action.
*/
template<class Goal, class Feedback> class Action {
@@ -59,151 +70,172 @@
protected:
/**
- * @brief This method is called on receipt of a new goal. The derived class will implement this method to
- * pursue the goal.
- * @param Goal The goal to accomplish.
- * @see notifyActivated
+ * @brief Blocking, user specified code for the action. This function should check periodically if isPreemptRequested is true, and if so, exit.
+ * @param goal The goal message currently associated with the controller.
+ * @param feedback The feedback message. At the end of the function, this will be published.
*/
- virtual void handleActivate(const Goal& goalMsg) = 0;
+ virtual ResultStatus execute(const Goal& goal, Feedback& feedback) = 0;
/**
- * @brief This method is called to preempt execution of a goal. The derived class must terminate all activiity
- * pursuing an active goal. As soon as we return from this call, the actor will be deactivated.
- * @see notifyPreempted
+ * @brief Allows the user to check if the action is active. This is primarily used to condition behavior
+ * for low duty cycles when the action is inactive
+ * @return True if the action is active, false otherwise.
*/
- virtual void handlePreempt() = 0;
+ bool isActive() const { return _status.value == _status.ACTIVE; }
+
+ /**
+ * @brief Called by the user to check if the action has a new goal.
+ * @return True if a request to preempt is made, false otherwise.
+ */
+ bool isPreemptRequested() const { return _preempt_request; }
/**
- * @brief A method call periodically - allows the action to provide intermediate feedback.
+ * @brief Called by the user when a new feedback message is ready to be published.
+ * @param feedback The feedback message.
*/
- virtual void handleUpdate(Feedback& feedback){}
+ void update(const Feedback& feedback) {
+ // Update local feedback state
+ _feedback.lock();
+ _feedback = feedback;
+ _feedback.unlock();
+ makeCallback(_status, _goal, feedback);
+ }
+
public:
/**
- * @brief This method is called on receipt of a new goal. The derived class will implement this method to
- * pursue the goal. The actor we be activated as soon as notifyActivated is called on the actioncontainer.
- * @param Goal The goal to accomplish.
- * @see notifyActivated
+ * @brief Accessor for the action name
*/
- void activate(const Goal& goal){
- ROS_DEBUG("%s received request to activate.", getName().c_str());
+ const std::string& getName() const { return _name; }
- if(!isActive()){
- _goal = goal;
- handleActivate(_goal);
- }
- }
-
/**
- * @brief This method is called to preempt execution of a goal. The derived class must terminate all activiity
- * pursuing an active goal. The actor will be deactivated as soon as notifyPreempted is called on the actioncontainer.
- * @see notifyPreempted
+ * @brief Preempts the controller. Blocks until it preempts.
*/
- void preempt(){
- ROS_DEBUG("%s received request to preempt.", getName().c_str());
- if(isActive())
- handlePreempt();
+ void preempt() {
+ _preempt_request = true;
+ ROS_DEBUG("[%s]Preempt requested\n", getName().c_str());
+ while(isActive()) {
+ ros::Duration d; d.fromSec(0.001);
+ d.sleep();
+ }
+ ROS_DEBUG("[%s]Preempt achieved\n", getName().c_str());
+ _preempt_request = false;
}
/**
- * @brief A call made periodically to provide an opportunity to execute. It will delegate to a subclass to
- * allow feedback to be written.
+ * @brief Activates the controller.
*/
- void update(){
+ void activate(const Goal& goal) {
+ if (isActive()) {
+ ROS_DEBUG("[%s]New goal forcing preemption of current active goal\n", getName().c_str());
+ preempt();
+ }
- // Allow for action to update
- if(isActive())
- handleUpdate(_feedback);
-
- // Always post a state update
- makeCallback(_status, _goal, _feedback);
+ // Set the goal and activate
+ ROS_DEBUG("[%s]Setting new goal.", getName().c_str());
+ _goal.lock();
+ _goal = goal;
+ _goal.unlock();
+ _status.value = _status.ACTIVE;
+ update(Feedback());
}
/**
- * @brief Accessor for the action name
- */
- const std::string& getName() const { return _name; }
-
- /**
* @brief Connect the action to a container for handling outbound messages
* @todo Implement with a functor object and bost bind perhaps
*/
void connect(const boost::function< void(const ActionStatus&, const Goal&, const Feedback&) >& callback){ _callback = callback; }
- protected:
-
- /** The notification methods below are used by derived action classes to generate state transition events **/
-
/**
- * @brief An action will call this method when it has been activated successfully
+ * @brief Call to terminate an action - prevent it from runLoopning again.
*/
- void notifyActivated(){
- ROS_DEBUG("%s activated.", getName().c_str());
+ void terminate() {
+ _terminated = true;
- _status.lock();
- if(!isActive()){
- _status.value = ActionStatus::ACTIVE;
- makeCallback(_status, _goal, _feedback);
+ if(isActive()){
+ preempt();
}
- _status.unlock();
}
/**
- * @brief An action will call this method when it has completed successfully.
- * @param Feedback to provide in the state update
+ * @brief Called by an external client to publish an update
*/
- void notifySucceeded(const Feedback& feedback){
- ROS_DEBUG("%s completed successfully.", getName().c_str());
- handleDeactivation(feedback, ActionStatus::SUCCESS);
+ void publish(){
+ makeCallback(_status, _goal, _feedback);
}
- /**
- * @brief An action will call this method when it has aborted of its own volition
- * @param Feedback to provide in the state update
- */
- void notifyAborted(const Feedback& feedback){
- ROS_DEBUG("%s aborted.", getName().c_str());
- handleDeactivation(feedback, ActionStatus::ABORTED);
- }
+ protected:
/**
- * @brief An action will call this method when it has successfully been preempted
- * @param Feedback to provide in the state update
- */
- void notifyPreempted(const Feedback& feedback){
- ROS_DEBUG("%s preempted.", getName().c_str());
- handleDeactivation(feedback, ActionStatus::PREEMPTED);
- }
-
- /**
* @brief Constructor
* @param name The action name
*/
Action(const std::string& name)
- : _name(name), _callback(NULL) {
+ : _name(name), _preempt_request(false), _result_status(SUCCESS), _terminated(false), _action_thread(NULL), _callback(NULL){
_status.value = ActionStatus::UNDEFINED;
+ _action_thread = new boost::thread(boost::bind(&Action<Goal, Feedback>::runLoop, this));
}
- virtual ~Action(){}
+ virtual ~Action(){
+ terminate();
+ _action_thread->join();
+ delete _action_thread;
+ }
+
/**
- * @brief True if the action is active, false otherwise
+ * @brief Called by the derived class to deactivate the node. Used when actions leverage call
+ * backs for processing. In that case the derived class will have to implement a busy loop in the execute
+ * method.
*/
- bool isActive() const {
- return _status.value == ActionStatus::ACTIVE;
+ void deactivate(const ResultStatus& result_status, const Feedback& feedback){
+ _result_status = result_status;
+ _status.value = result_status;
+ _feedb...
[truncated message content] |
