Go to the documentation of this file.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
00033
00034
00035
00036 #include <trajectory_execution_ros/trajectory_execution_monitor_ros.h>
00037 #include <planning_scene_monitor/planning_scene_monitor.h>
00038 #include <csignal>
00039 #include <gtest/gtest.h>
00040 #include <pr2_mechanism_msgs/ListControllers.h>
00041
00042 boost::shared_ptr<trajectory_execution_ros::TrajectoryExecutionMonitorRos> emon;
00043 boost::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
00044 ros::ServiceClient lister_service;
00045
00046 void sigHandler(int x) {
00047 emon.reset();
00048 exit(0);
00049 }
00050
00051 void getRunningControllerMap(std::map<std::string, bool>& controller_map) {
00052 controller_map.clear();
00053
00054 pr2_mechanism_msgs::ListControllers::Request req;
00055 pr2_mechanism_msgs::ListControllers::Response res;
00056
00057 if(!lister_service.call(req, res)) {
00058 ROS_WARN_STREAM("Something wrong with lister service");
00059 return;
00060 }
00061 for(unsigned int i = 0; i < res.controllers.size(); i++) {
00062 controller_map[res.controllers[i]] = (res.state[i] == "running");
00063 }
00064 }
00065
00066 TEST(TrajectoryExecutionTests, loadUnload) {
00067
00068 emon.reset(new trajectory_execution_ros::TrajectoryExecutionMonitorRos(planning_scene_monitor_->getPlanningScene()->getRobotModel()));
00069 std::map<std::string, bool> initial_map = emon->getOriginalControllerConfiguration();
00070 emon.reset();
00071 std::map<std::string, bool> after_map;
00072 getRunningControllerMap(after_map);
00073 ASSERT_EQ(after_map.size(), initial_map.size());
00074 std::map<std::string, bool>::iterator it2 = after_map.begin();
00075 for(std::map<std::string, bool>::iterator it = initial_map.begin();
00076 it != initial_map.end();
00077 it++, it2++) {
00078 EXPECT_EQ(it->first, it2->first);
00079 EXPECT_EQ(it->second, it2->second);
00080 }
00081 }
00082
00083 TEST(TrajectoryExecutionTests, switchRestore) {
00084 emon.reset(new trajectory_execution_ros::TrajectoryExecutionMonitorRos(planning_scene_monitor_->getPlanningScene()->getRobotModel()));
00085 std::map<std::string, bool> initial_map = emon->getOriginalControllerConfiguration();
00086
00087 EXPECT_TRUE(emon->getCurrentController("arms") == "r_arm_controller" ||
00088 emon->getCurrentController("arms") == "l_arm_controller");
00089 emon->switchAssociatedStopStartControllers("arms", "both_arms_controller");
00090
00091 EXPECT_EQ(emon->getCurrentController("arms"), "both_arms_controller");
00092 EXPECT_EQ(emon->getCurrentController("right_arm"), "both_arms_controller");
00093 EXPECT_EQ(emon->getCurrentController("left_arm"), "both_arms_controller");
00094
00095 emon->switchAssociatedStopStartControllers("right_arm", "r_arm_controller");
00096
00097 EXPECT_EQ(emon->getCurrentController("arms"), "r_arm_controller");
00098 EXPECT_EQ(emon->getCurrentController("left_arm"), "l_arm_controller");
00099
00100 emon->restoreOriginalControllers();
00101
00102 std::map<std::string, bool> after_map;
00103 getRunningControllerMap(after_map);
00104 ASSERT_EQ(after_map.size(), initial_map.size());
00105 std::map<std::string, bool>::iterator it2 = after_map.begin();
00106 for(std::map<std::string, bool>::iterator it = initial_map.begin();
00107 it != initial_map.end();
00108 it++, it2++) {
00109 EXPECT_EQ(it->first, it2->first);
00110 EXPECT_EQ(it->second, it2->second);
00111 }
00112 }
00113
00114 int main(int argc, char **argv)
00115 {
00116 ros::init(argc, argv, "trajectory_execution_tests", ros::init_options::NoSigintHandler);
00117 testing::InitGoogleTest(&argc, argv);
00118
00119 signal(SIGINT, sigHandler);
00120 signal(SIGTERM, sigHandler);
00121
00122 ros::NodeHandle nh;
00123
00124 ros::service::waitForService("/pr2_controller_manager/list_controllers");
00125
00126 planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));
00127 lister_service = nh.serviceClient<pr2_mechanism_msgs::ListControllers>("/pr2_controller_manager/list_controllers", true);
00128
00129 int ret = RUN_ALL_TESTS();
00130
00131 emon.reset();
00132
00133 return ret;
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144 }