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