I have a node that creates a motion plan of the type moveit_msgs::RobotTrajectory. I want to hold this motion plan for future use (i.e., load it later to "execute" it). However, I only want to save one of these motion plans to a file at one time: i.e., I want my .bag file to only ever contain one file so when I load it, I will only load one motion plan. Any suggestions for ways to do this or better ways to accomplish the same end? This concept is easy to implement in matlab by just overwriting files.
Here is some of my code...
void automaticCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
ros::AsyncSpinner spinner(4);
spinner.start();
std::string jointgroup = "sia5dArm";
move_group_interface::MoveGroup group(jointgroup);
moveit::planning_interface::MoveGroup::Plan my_plan;
group.setPoseTarget(*msg);
bool success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
if (success == true)
{
std::vector trajectory_points;
trajectory_points = my_plan.trajectory_.joint_trajectory.points;
std::vector::size_type points_number = trajectory_points.size();
std::vector motionPlan_msg;
std::vector motionPlan_times;
for (unsigned i = 0; i::size_type joints_number = trajectory_points[i].positions.size();
double trajectory_time = ros::Duration(trajectory_points[i].time_from_start).toSec();
motionPlan_times.push_back(trajectory_time);
for (unsigned j = 0; j("motionPlan",1000);
motionPlan_pub.publish(motionPlan_position);
ros::spin();
}
ros::waitForShutdown();
}
(A side note... if anyone can see anything wrong with my spinners, please lmk. It keeps throwing an error telling me I need multithreaded spinners, but I when I try that nothing publishes.)
↧