41 int main(
int argc,
char** argv)
43 ros::init(argc, argv,
"test_trajectory_execution_manager");
53 if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1,
"basej")))
57 if (!tem.ensureActiveController(
"arms"))
61 if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1,
"rj2")))
65 if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1,
"lj1")))
69 if (!tem.ensureActiveController(
"left_arm_head"))
72 if (!tem.ensureActiveController(
"arms"))
77 if (!tem.waitForExecution())
80 moveit_msgs::RobotTrajectory traj1;
81 traj1.joint_trajectory.joint_names.push_back(
"rj1");
82 traj1.joint_trajectory.points.resize(1);
83 traj1.joint_trajectory.points[0].positions.push_back(0.0);
87 moveit_msgs::RobotTrajectory traj2 = traj1;
88 traj2.joint_trajectory.joint_names.push_back(
"lj2");
89 traj2.joint_trajectory.points[0].positions.push_back(1.0);
90 traj2.multi_dof_joint_trajectory.joint_names.push_back(
"basej");
91 traj2.multi_dof_joint_trajectory.points.resize(1);
92 traj2.multi_dof_joint_trajectory.points[0].transforms.resize(1);
97 traj1.multi_dof_joint_trajectory = traj2.multi_dof_joint_trajectory;
101 if (!tem.executeAndWait())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const CurrentStateMonitorPtr & getStateMonitor() const
Get the stored instance of the stored current state monitor.
PlanningSceneMonitor Subscribes to the topic planning_scene.
int main(int argc, char **argv)
ROSCPP_DECL void waitForShutdown()