Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23331] pkg/trunk
|
From: <tpr...@us...> - 2009-08-29 03:34:44
|
Revision: 23331
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23331&view=rev
Author: tpratkanis
Date: 2009-08-29 03:34:34 +0000 (Sat, 29 Aug 2009)
Log Message:
-----------
Stub out handle detection because it fails randomly, also add debug info.
Modified Paths:
--------------
pkg/trunk/demos/door_demos_gazebo/launch/pr2_door_test.launch
pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
Modified: pkg/trunk/demos/door_demos_gazebo/launch/pr2_door_test.launch
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/launch/pr2_door_test.launch 2009-08-29 03:33:12 UTC (rev 23330)
+++ pkg/trunk/demos/door_demos_gazebo/launch/pr2_door_test.launch 2009-08-29 03:34:34 UTC (rev 23331)
@@ -1,7 +1,9 @@
<launch>
<include file="$(find door_demos_gazebo)/launch/pr2_and_door.launch"/>
<include file="$(find milestone2_actions)/milestone2.launch" />
- <node pkg="doors_core" type="test_executive" output="screen" />
+ <node pkg="doors_core" type="test_executive" output="screen">
+ <param name="stub_handle_detector" value="true" type="bool" />
+ </node>
<!--<node pkg="rosrecord" type="rosrecord" args=" -f door_demos_test.bag " />-->
<test test-name="door_demos_test_open_door" pkg="door_demos_gazebo" type="door_demo_test_exec_test.py" time-limit="1000" retry="2" />
Modified: pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-08-29 03:33:12 UTC (rev 23330)
+++ pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-08-29 03:34:34 UTC (rev 23331)
@@ -54,7 +54,13 @@
using namespace std;
using namespace door_functions;
+FILE* output = NULL;
+void writeString(std::string txt) {
+ if (output)
+ fprintf(output, "%s\n", txt.c_str());
+ ROS_INFO("[Test Executive] %s", txt.c_str());
+}
// -----------------------------------
@@ -69,8 +75,15 @@
ros::Node node("test_executive");
boost::thread* thread;
- ROS_INFO("Test executive started");
+ writeString("Test executive started");
+ bool use_stub = false;
+ node.param<bool>("~stub_handle_detector", use_stub, use_stub);
+
+ if (use_stub) {
+ writeString("Using the stub door handle detector.");
+ }
+
door_msgs::Door prior_door;
prior_door.frame_p1.x = 1.0;
prior_door.frame_p1.y = -0.5;
@@ -109,7 +122,7 @@
Duration timeout_medium = Duration().fromSec(10.0);
Duration timeout_long = Duration().fromSec(40.0);
- ROS_INFO("Starting acion clients");
+ writeString("Starting action clients");
robot_actions::ActionClient<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> tuck_arm("safety_tuck_arms");
robot_actions::ActionClient<pr2_robot_actions::SwitchControllers, pr2_robot_actions::SwitchControllersState, std_msgs::Empty> switch_controllers("switch_controllers");
robot_actions::ActionClient<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door> detect_door("detect_door");
@@ -127,61 +140,94 @@
door_msgs::Door backup_door;
std::ostringstream os; os << prior_door;
- ROS_INFO("before %s", os.str().c_str());
+ writeString("before " + os.str());
// tuck arm
- ROS_INFO("begining tuck arms");
+ writeString("begining tuck arms");
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.start_controllers.push_back("r_arm_joint_trajectory_controller");
if (switch_controllers.execute(switchlist, empty, timeout_medium) != robot_actions::SUCCESS) return -1;
if (tuck_arm.execute(empty, empty, timeout_medium) != robot_actions::SUCCESS) return -1;
- ROS_INFO("done tuck arms");
+ writeString("done tuck arms");
// detect door
- ROS_INFO("begining detect door");
+ writeString("begining detect door");
door_msgs::Door res_detect_door;
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.start_controllers.push_back("laser_tilt_controller");
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
- ROS_INFO("door detect");
+ writeString("door detect");
while (detect_door.execute(prior_door, tmp_door, timeout_long) != robot_actions::SUCCESS);
res_detect_door = tmp_door;
std::ostringstream os2; os2 << res_detect_door;
- ROS_INFO("detect door %s", os2.str().c_str());
+ writeString("detect door " + os2.str());
// detect handle if door is latched
- ROS_INFO("begining detect handle");
+ writeString("begining detect handle");
door_msgs::Door res_detect_handle;
+ // hard coded stub
+ if (use_stub) {
+ res_detect_handle.header.stamp = ros::Time::now();
+ res_detect_handle.header.frame_id = "odom_combined";
+ res_detect_handle.frame_p1.x = 1.20534;
+ res_detect_handle.frame_p1.y = -0.344458;
+ res_detect_handle.frame_p1.z = 0;
+ res_detect_handle.frame_p2.x = 1.20511;
+ res_detect_handle.frame_p2.y = 0.651193;
+ res_detect_handle.frame_p2.z = 0;
+ res_detect_handle.door_p1.x = 1.20587;
+ res_detect_handle.door_p1.y = -0.344458;
+ res_detect_handle.door_p1.z = 0;
+ res_detect_handle.door_p2.x = 1.20511;
+ res_detect_handle.door_p2.y = 0.651193;
+ res_detect_handle.door_p2.z = 0;
+ res_detect_handle.handle.x = 1.16614;
+ res_detect_handle.handle.y = -0.114199;
+ res_detect_handle.handle.z = 0.853391;
+ res_detect_handle.travel_dir.x = 1;
+ res_detect_handle.travel_dir.y = 0.000233471;
+ res_detect_handle.travel_dir.z = 0;
+ res_detect_handle.latch_state = 2;
+ res_detect_handle.hinge = 2;
+ res_detect_handle.rot_dir = 2;
+ }
+
bool open_by_pushing = false;
if (res_detect_door.latch_state == door_msgs::Door::UNLATCHED)
open_by_pushing = true;
if (!open_by_pushing){
- ROS_INFO("Not opening by pushing - pointing head at handle and detecting");
+ writeString("Not opening by pushing - pointing head at handle and detecting");
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.start_controllers.push_back("head_controller");
switchlist.start_controllers.push_back("head_pan_joint_position_controller");
switchlist.start_controllers.push_back("head_tilt_joint_position_controller");
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
- while (detect_handle.execute(res_detect_door, tmp_door, timeout_long) != robot_actions::SUCCESS);
- res_detect_handle = tmp_door;
+ if (!use_stub) {
+ writeString("Using Real Handle Detector");
+ while (detect_handle.execute(res_detect_door, tmp_door, timeout_long) != robot_actions::SUCCESS);
+ res_detect_handle = tmp_door;
+ } else {
+ writeString("Using Stub Handle Detector");
+ }
std::ostringstream os3; os3 << res_detect_handle;
- ROS_INFO("detect handle %s", os3.str().c_str());
+ writeString("detect handle " + os3.str());
}
// approach door
geometry_msgs::PoseStamped goal_msg;
tf::poseStampedTFToMsg(getRobotPose(res_detect_door, -0.6), goal_msg);
- ROS_INFO("move to pose %f, %f, %f", goal_msg.pose.position.x, goal_msg.pose.position.y, goal_msg.pose.position.z);
+ std::ostringstream target; target << goal_msg.pose.position.x << ", " << goal_msg.pose.position.y << ", " << goal_msg.pose.position.z;
+ writeString("move to pose " + target.str() + ".");
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
- while (move_base_local.execute(goal_msg, goal_msg) != robot_actions::SUCCESS) {cout << "re-trying move base local" << endl;};
- ROS_INFO("door approach finished");
+ while (move_base_local.execute(goal_msg, goal_msg) != robot_actions::SUCCESS) {writeString("re-trying move base local");};
+ writeString("door approach finished");
// touch door
if (open_by_pushing){
- ROS_INFO("Open by pushing");
+ writeString("Open by pushing");
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_joint_trajectory_controller");
switchlist.start_controllers.push_back("r_gripper_effort_controller");
@@ -191,10 +237,10 @@
switchlist.start_controllers.push_back("r_arm_constraint_cartesian_wrench_controller");
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
if (touch_door.execute(res_detect_door, tmp_door, timeout_long) != robot_actions::SUCCESS) return -1;
- ROS_INFO("Door touched");
+ writeString("Door touched");
// push door in separate thread
- ROS_INFO("Open by pushing in seperate thread");
+ writeString("Open by pushing in seperate thread");
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_constraint_cartesian_trajectory_controller");
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
@@ -203,7 +249,7 @@
&push_door, res_detect_door, tmp_door, timeout_long));
}
else{
- ROS_INFO("Open by grasping and unlatching");
+ writeString("Open by grasping and unlatching");
// grasp handle
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_joint_trajectory_controller");
@@ -215,7 +261,7 @@
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
if (grasp_handle.execute(res_detect_handle, tmp_door, timeout_long) != robot_actions::SUCCESS) return -1;
- ROS_INFO("Unlatching");
+ writeString("Unlatching");
// unlatch handle
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_constraint_cartesian_trajectory_controller");
@@ -225,7 +271,7 @@
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
if (unlatch_handle.execute(res_detect_handle, tmp_door, timeout_long) != robot_actions::SUCCESS) return -1;
- ROS_INFO("Open goor in seprate thread");
+ writeString("Open goor in seprate thread");
// open door in separate thread
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
@@ -241,14 +287,14 @@
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
std::ostringstream os4; os4 << res_detect_handle;
- ROS_INFO("Moving through door with door message %s", os4.str().c_str());
+ writeString("Moving through door with door message " + os4.str());
if (move_base_door.execute(res_detect_door, tmp_door) != robot_actions::SUCCESS)
{
move_thru_success = false;
backup_door = res_detect_door;
};
- ROS_INFO("Preempt openning thread and join");
+ writeString("Preempt openning thread and join");
// preempt open/push door
if (open_by_pushing)
push_door.preempt();
@@ -260,7 +306,7 @@
// release handle
if (!open_by_pushing){
- ROS_INFO("Releasing handle");
+ writeString("Releasing handle");
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_cartesian_tff_controller");
switchlist.start_controllers.push_back("r_arm_constraint_cartesian_trajectory_controller");
@@ -271,7 +317,7 @@
}
// tuck arm
- ROS_INFO("Tucking arm");
+ writeString("Tucking arm");
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_constraint_cartesian_trajectory_controller");
switchlist.stop_controllers.push_back("r_arm_constraint_cartesian_pose_controller");
@@ -284,7 +330,7 @@
if(!move_thru_success)
{
std::ostringstream os5; os5 << backup_door;
- ROS_INFO("Move through failed, using backup_door %s", os5.str().c_str());
+ writeString("Move through failed, using backup_door " + os5.str());
backup_door.travel_dir.x = -1.0;
backup_door.travel_dir.y = 0.0;
if (move_base_door.execute(backup_door, tmp_door) != robot_actions::SUCCESS)
@@ -294,17 +340,18 @@
// go to the last location
double X = 27.3095662355 + 3 - 25.7, Y = 25.8414441058 - 25.7;
- ROS_INFO("Moving to %f %f", X, Y);
+ std::ostringstream os6; os6 << "Moving to" << X << "," << Y;
+ writeString(os6.str());
goal_msg.header.frame_id = "/odom_combined";
goal_msg.pose.position.x = X;
goal_msg.pose.position.y = Y;
goal_msg.pose.position.z = 0;
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
- while (move_base_local.execute(goal_msg, goal_msg) != robot_actions::SUCCESS) {cout << "re-trying move base local" << endl;};
+ while (move_base_local.execute(goal_msg, goal_msg) != robot_actions::SUCCESS) {writeString("re-trying move base local");};
// stop remaining controllers
- ROS_INFO("Stop controllers");
+ writeString("Stop controllers");
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("laser_tilt_controller");
switchlist.stop_controllers.push_back("head_controller");
@@ -312,6 +359,6 @@
switchlist.stop_controllers.push_back("r_arm_joint_trajectory_controller");
if (switch_controllers.execute(switchlist, empty, timeout_medium) != robot_actions::SUCCESS) return -1;
- ROS_INFO("Done");
+ writeString("Done");
return (0);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
