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
00037 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
00038 #include <moveit/robot_model_loader/robot_model_loader.h>
00039 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00040
00041 int main(int argc, char** argv)
00042 {
00043 ros::init(argc, argv, "test_trajectory_execution_manager");
00044
00045 ros::AsyncSpinner spinner(1);
00046 spinner.start();
00047
00048 robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader);
00049 planning_scene_monitor::PlanningSceneMonitor psm(rml);
00050 trajectory_execution_manager::TrajectoryExecutionManager tem(rml->getModel(), psm.getStateMonitor(), true);
00051
00052 std::cout << "1:\n";
00053 if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1, "basej")))
00054 ROS_ERROR("Fail!");
00055
00056 std::cout << "2:\n";
00057 if (!tem.ensureActiveController("arms"))
00058 ROS_ERROR("Fail!");
00059
00060 std::cout << "3:\n";
00061 if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1, "rj2")))
00062 ROS_ERROR("Fail!");
00063
00064 std::cout << "4:\n";
00065 if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1, "lj1")))
00066 ROS_ERROR("Fail!");
00067
00068 std::cout << "5:\n";
00069 if (!tem.ensureActiveController("left_arm_head"))
00070 ROS_ERROR("Fail!");
00071 std::cout << "6:\n";
00072 if (!tem.ensureActiveController("arms"))
00073 ROS_ERROR("Fail!");
00074
00075
00076 tem.execute();
00077 if (!tem.waitForExecution())
00078 ROS_ERROR("Fail!");
00079
00080 moveit_msgs::RobotTrajectory traj1;
00081 traj1.joint_trajectory.joint_names.push_back("rj1");
00082 traj1.joint_trajectory.points.resize(1);
00083 traj1.joint_trajectory.points[0].positions.push_back(0.0);
00084 if (!tem.push(traj1))
00085 ROS_ERROR("Fail!");
00086
00087 moveit_msgs::RobotTrajectory traj2 = traj1;
00088 traj2.joint_trajectory.joint_names.push_back("lj2");
00089 traj2.joint_trajectory.points[0].positions.push_back(1.0);
00090 traj2.multi_dof_joint_trajectory.joint_names.push_back("basej");
00091 traj2.multi_dof_joint_trajectory.points.resize(1);
00092 traj2.multi_dof_joint_trajectory.points[0].transforms.resize(1);
00093
00094 if (!tem.push(traj2))
00095 ROS_ERROR("Fail!");
00096
00097 traj1.multi_dof_joint_trajectory = traj2.multi_dof_joint_trajectory;
00098 if (!tem.push(traj1))
00099 ROS_ERROR("Fail!");
00100
00101 if (!tem.executeAndWait())
00102 ROS_ERROR("Fail!");
00103
00104 ros::waitForShutdown();
00105
00106 return 0;
00107 }