pr2 with camera point to point

pr2_positions.cpp

#include <moveit/move_group_interface/move_group.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>

int main(int argc, char **argv){
  float sleepTime = 10.0;
  float computeTime = 10.0;

  geometry_msgs::TransformStamped pr2TransformStamp;

  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.28;
  target_pose1.position.y = -0.7;
  target_pose1.position.z = 1.0;

  ros::init(argc, argv, "pr2_positions");
  ros::NodeHandle node_handle;  
  
  ros::Rate rate(50);
  int counter = 0;
  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

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

  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  
  moveit::planning_interface::MoveGroup group("right_arm");

  // Print the name of the reference frame for this robot.
  ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
  
  // Print the name of the end-effector link for this group.
  ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());

  group.setPlanningTime(computeTime); 

  // Set target for group
  group.setPoseTarget(target_pose1);

  // Call the planner to compute and visualize plan
  moveit::planning_interface::MoveGroup::Plan my_plan;
  bool success = group.plan(my_plan);

  ROS_INFO("Visualizing plan %s",success?"":"FAILED");    
  /* Sleep sleepTimegive Rviz time to visualize plan */
  sleep(sleepTime);

  group.execute(my_plan);

  sleep(sleepTime);

  while(counter < 200){
    try{
      pr2TransformStamp = tfBuffer.lookupTransform("base_link",
                          "r_wrist_roll_link", ros::Time(0));
    }
    catch(tf2::TransformException &ex){
      ROS_WARN("%s", ex.what());
    }

    ROS_INFO_STREAM("pr2TransformStamp x " 
      << pr2TransformStamp.transform.translation.x);

    counter++;
  }

  // Create the camera object to attach
  moveit_msgs::CollisionObject collision_object;
  std::vector<moveit_msgs::CollisionObject> collision_object_vec;
  collision_object.header.frame_id = group.getPlanningFrame();

  collision_object.id = "camera";

  // Define the shape
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 0.03;
  primitive.dimensions[1] = 0.05;
  primitive.dimensions[2] = 0.05;

  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = pr2TransformStamp.transform.translation.x+0.216;
  box_pose.position.y = pr2TransformStamp.transform.translation.y;
  box_pose.position.z = pr2TransformStamp.transform.translation.z+0.05;

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  collision_object_vec.push_back(collision_object);

  ROS_INFO("Add camera to model");  
  planning_scene_interface.addCollisionObjects(collision_object_vec); 

  ROS_INFO("Attach camera to model");

  group.attachObject(collision_object.id);
  sleep(sleepTime);

  geometry_msgs::Pose target_pose2;
  target_pose2.orientation.w = 1.0;
  target_pose2.position.x = 0.28;
  target_pose2.position.y = -0.75;
  target_pose2.position.z = 1.0;

  // Set target for group
  group.setPoseTarget(target_pose2);

  // Call the planner to compute and visualize plan
  // moveit::planning_interface::MoveGroup::Plan my_plan2;
  success = group.plan(my_plan);

  ROS_INFO("Visualizing plan %s",success?"":"FAILED");    
  /* Sleep sleepTimegive Rviz time to visualize plan */
  sleep(sleepTime);

  group.execute(my_plan);

  target_pose2.orientation.w = 1.0;
  target_pose2.position.x = 0.28;
  target_pose2.position.y = -0.70;
  target_pose2.position.z = 1;

  // Set target for group
  group.setPoseTarget(target_pose2);

  // Call the planner to compute and visualize plan
  // moveit::planning_interface::MoveGroup::Plan my_plan2;
  success = group.plan(my_plan);

  ROS_INFO("Visualizing plan %s",success?"":"FAILED");    
  /* Sleep sleepTimegive Rviz time to visualize plan */
  sleep(sleepTime);

  group.execute(my_plan);

  target_pose2.orientation.w = 1.0;
  target_pose2.position.x = 0.33;
  target_pose2.position.y = -0.70;
  target_pose2.position.z = 1.0;

  // Set target for group
  group.setPoseTarget(target_pose2);

  // Call the planner to compute and visualize plan
  // moveit::planning_interface::MoveGroup::Plan my_plan2;
  success = group.plan(my_plan);

  ROS_INFO("Visualizing plan %s",success?"":"FAILED");    
  /* Sleep sleepTimegive Rviz time to visualize plan */
  sleep(sleepTime);

  group.execute(my_plan);

  ros::shutdown();  
  return 0;
}