trajectory_reverse.cpp

#include <ros/ros.h>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>

int main(int argc, char **argv) {
  ros::init(argc, argv, "Foobar");

  ros::AsyncSpinner spinner(1);
  spinner.start();

  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");

  ros::Publisher pub_t1= nh.advertise<trajectory_msgs::JointTrajectory>("traj_normal", 1, true);
  ros::Publisher pub_t2= nh.advertise<trajectory_msgs::JointTrajectory>("traj_reversed", 1, true);

  moveit::planning_interface::MoveGroupInterface mgi("manipulator");

  moveit::planning_interface::MoveGroupInterface::Plan plan;

  robot_state::RobotStatePtr state;
  do {
   state = mgi.getCurrentState();
        ROS_INFO("waiting for current state");
  } while (!state);

  do {
        mgi.setRandomTarget();
        ROS_INFO("choose random target");
  } while(!mgi.plan(plan));

  ROS_INFO("got trajectory");

  robot_trajectory::RobotTrajectory traj(mgi.getRobotModel(), "manipulator");
  traj.setRobotTrajectoryMsg(*state, plan.trajectory_);

  {
         moveit_msgs::RobotTrajectory t;
         traj.getRobotTrajectoryMsg(t);
         pub_t1.publish(t.joint_trajectory);
  }

  traj.reverse();

  {
         moveit_msgs::RobotTrajectory t;
         traj.getRobotTrajectoryMsg(t);
         pub_t2.publish(t.joint_trajectory);
  }

  ros::waitForShutdown();

  return 0;
}