Ask Your Question
0

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

asked 2018-07-11 18:35:55 -0600

Levi_Manring gravatar image

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.)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-07-11 22:41:24 -0600

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();
edit flag offensive delete link more

Comments

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?

Levi_Manring gravatar imageLevi_Manring ( 2018-07-12 10:37:17 -0600 )edit
1

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_.

BryceWilley gravatar imageBryceWilley ( 2018-07-12 10:46:49 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-07-11 18:35:55 -0600

Seen: 89 times

Last updated: Jul 11 '18