How can I record a single message in a rosbag to reserve to reload for future use?

Question

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_msgs::JointTrajectoryPoint> trajectory_points;
    trajectory_points = my_plan.trajectory_.joint_trajectory.points;

    std::vector<int>::size_type points_number = trajectory_points.size();
    std::vector<double> motionPlan_msg;
    std::vector<double> motionPlan_times;

    for (unsigned i = 0; i<points_number; i++)
    {
       std::vector<int>::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<joints_number; j++)
       {
          motionPlan_msg.push_back(trajectory_points[i].positions[j]);
        } 
    }

sensor_msgs::JointState motionPlan_position;
motionPlan_position.position = motionPlan_msg;
motionPlan_position.effort = motionPlan_times;

ros::NodeHandle n;
ros::Publisher motionPlan_pub = n.advertise<sensor_msgs::JointState>("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.)

---

Originally posted by Levi_Manring on ROS Answers with karma: 5 on 2018-07-11

Post score: 0

Answer 1

Rosbag has a C++ API that should allow you to open a new rosbag file, write the single motion plan, and close it. Code would be something like:

// Open new rosbag.
rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Write);

// Write the message.
bag.addMessage("my_lonely_topic", motionPlan_position);

// Close the bag:
bag.close();

---

Originally posted by BryceWilley with karma: 711 on 2018-07-11

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by Levi_Manring on 2018-07-12:
so this works for motionPlan_position, but what I actually want to save to the .bag file is my_plan, which is a Robot Trajectory. However, this doesn't work. Any ideas why?

Comment by BryceWilley on 2018-07-12:
Ah, I see. So my_plan is just the amount of time it took to generate the plan, and the robot_trajectory itself. You should be able to save my_plan.trajectory_.