Visualize robot trajectory in Rviz

edit

  //initial setup

  ros::Publisher pub_traj= node_handle.advertise<moveit_msgs::DisplayTrajectory>("display_planned_path", 1);   
  ROS_INFO_STREAM("Initializing objects");
  robot_model_loader::RobotModelLoader model_loader(robot_description);
  robot_model::RobotModelPtr kinematic_model = model_loader.getModel();
  robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
  planning_scene_monitor::PlanningSceneMonitorPtr p_s_m(new planning_scene_monitor::PlanningSceneMonitor(robot_description));
  planning_scene::PlanningScenePtr p_s = p_s_m->getPlanningScene();

  const robot_state::JointModelGroup* joint_group = kinematic_model->getJointModelGroup("right_arm");
  const std::vector<std::string> joint_names = joint_group->getJointModelNames();
  std::vector<double> joint_values;
  kinematic_state->copyJointGroupPositions(joint_group, joint_values);

  //Preparing display trajectory message (initial state)
  ROS_INFO_STREAM("Popluating trajectory message");
  moveit_msgs::RobotState robot_state_msg;
  robot_state_msg.joint_state.name.resize(joint_names.size());
  robot_state_msg.joint_state.position.resize(joint_values.size());

  //Copying initial state elements into RobotState msg
  int i2 = 0;
  for(std::vector<double>::size_type i = 0; i < joint_values.size(); i++)
  {
    //ROS_INFO_STREAM("Joint " << joint_names[i2] << " has position " << joint_values[i2]);
    robot_state_msg.joint_state.name[i2] = joint_names[i2];
    robot_state_msg.joint_state.position[i2] = joint_values[i2];
    i2++;
  }

  //randomizing joint positions
  kinematic_state->setToRandomPositions(joint_group);

  //set trajectory for Rviz to display (just one state in the trajectory message)
  trajectory_msgs::JointTrajectory joint_trajectory;
  joint_trajectory.joint_names.resize(joint_names.size());
  joint_trajectory.points.resize(1);
  joint_trajectory.points[0].positions.resize(joint_values.size());

  //copying new random joint states into message
  for(std::size_t i = 0; i < joint_names.size(); i++)
  {
    ROS_INFO_STREAM("Joint " << joint_names[i2] << " has position " << joint_values[i2]);
    joint_trajectory.joint_names[i2] = joint_names[i2];
    joint_trajectory.points[0].positions[i2] = joint_values[i2];
    i2++;
  }

  joint_trajectory.points[0].time_from_start = ros::Duration(1.5);
  joint_trajectory.header.frame_id = "NaoH25V40";

  moveit_msgs::RobotTrajectory robot_trajectory;
  robot_trajectory.joint_trajectory = joint_trajectory;

  moveit_msgs::DisplayTrajectory display_trajectory;
  display_trajectory.model_id = p_s->getRobotModel()->getName();
  display_trajectory.trajectory_start = robot_state_msg;
  display_trajectory.trajectory.clear();
  display_trajectory.trajectory.push_back(robot_trajectory);

  pub_traj.publish(display_trajectory);