Revision history [back]

What I ended up doing was writing the motion plan to a .bag file, and then reading from it at a later point. The timing is so far working fine, so the problem is eliminated on that point. However, since I had to convert the plan to a RobotTrajectory message I need to still figure out how to execute the plan (commented here)

Writing:

    rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Write);
bag.write("my_plan_trajectory", ros::Time::now(), my_plan.trajectory_);
bag.close();

Reading:

rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Read);

std::vector<std::string> topics;
topics.push_back(std::string("my_plan_trajectory"));
rosbag::View view(bag, rosbag::TopicQuery(topics));

foreach(rosbag::MessageInstance const m, view)
{
    moveit_msgs::RobotTrajectory::ConstPtr s = m.instantiate<moveit_msgs::RobotTrajectory>();
    if (s != NULL)
    {
        ROS_INFO("executing command");
        //execute a trajectory
    }
}
bag.close();