00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <ros/ros.h>
00033 #include <gtest/gtest.h>
00034 #include <boost/thread.hpp>
00035 #include <pr2_plugs_msgs/VisionOutletDetectionAction.h>
00036 #include <pr2_plugs_msgs/VisionPlugDetectionAction.h>
00037
00038 #include <actionlib/client/simple_action_client.h>
00039 #include <actionlib/server/simple_action_server.h>
00040
00041 void spinThread()
00042 {
00043 ros::spin();
00044 }
00045
00046 TEST(ActionServerTest, vision_plug_detection)
00047 {
00048 ros::NodeHandle n;
00049 boost::thread spin_thread(&spinThread);
00050
00051 actionlib::SimpleActionClient<pr2_plugs_msgs::VisionPlugDetectionAction> ac("vision_plug_detection");
00052 pr2_plugs_msgs::VisionPlugDetectionGoal goal;
00053 goal.prior.header.stamp = ros::Time::now();
00054 goal.prior.header.frame_id = "base_link";
00055 goal.origin_on_right = false;
00056 goal.camera_name = "l_forearm_cam";
00057
00058 ASSERT_TRUE(ac.waitForServer(ros::Duration(100.0)));
00059
00060 ac.sendGoal(goal);
00061 ac.waitForResult();
00062 EXPECT_TRUE(ac.getState() == actionlib::SimpleClientGoalState::ABORTED);
00063 ros::shutdown();
00064 spin_thread.join();
00065
00066 }
00067
00068 TEST(ActionServerTest, vision_outlet_detection)
00069 {
00070 ros::NodeHandle n;
00071 boost::thread spin_thread(&spinThread);
00072
00073 actionlib::SimpleActionClient<pr2_plugs_msgs::VisionOutletDetectionAction> ac("vision_outlet_detection");
00074 pr2_plugs_msgs::VisionOutletDetectionGoal goal;
00075 goal.prior.header.stamp = ros::Time::now();
00076 goal.prior.header.frame_id = "base_link";
00077 goal.camera_name = "l_forearm_cam";
00078
00079 ASSERT_TRUE(ac.waitForServer(ros::Duration(100.0)));
00080
00081 ac.sendGoal(goal);
00082 ac.waitForResult();
00083 EXPECT_TRUE(ac.getState() == actionlib::SimpleClientGoalState::ABORTED);
00084 ros::shutdown();
00085 spin_thread.join();
00086
00087 }
00088
00089
00090 int main(int argc, char **argv){
00091 testing::InitGoogleTest(&argc, argv);
00092 ros::init(argc, argv, "vision_detect_outlet_and_plug_test");
00093 return RUN_ALL_TESTS();
00094
00095 }
00096