Testing stubs for named frames in MoveIt planning scene

gistfile1.txt

bool moveToCartPose(geometry_msgs::PoseStamped pose, std::string robot_name, bool wait, std::string end_effector_link, double velocity_scaling_factor)
{
  moveit::planning_interface::MoveGroupInterface::Plan myplan;
  moveit::planning_interface::MoveItErrorCode 
    success_plan = moveit_msgs::MoveItErrorCodes::FAILURE, 
    motion_done = moveit_msgs::MoveItErrorCodes::FAILURE;

  moveit::planning_interface::MoveGroupInterface* group_pointer;
  group_pointer = robotNameToMoveGroup(robot_name);
  
  group_pointer->clearPoseTargets();
  group_pointer->setStartStateToCurrentState();
  group_pointer->setMaxVelocityScalingFactor(velocity_scaling_factor);  // TODO: Check if this works
  if (end_effector_link == "")  // Define default end effector link explicitly
  {
    if (robot_name == "a_bot") group_pointer->setEndEffectorLink("a_bot_gripper_tip_link");
    else group_pointer->setEndEffectorLink(robot_name + "_robotiq_85_tip_link");
  }
  else group_pointer->setEndEffectorLink(end_effector_link);
  group_pointer->setPoseTarget(pose);

  ROS_INFO_STREAM("Planning motion for robot " << robot_name << " and EE link " << end_effector_link + "_tip_link, to pose:");
  ROS_INFO_STREAM(pose.pose.position.x << ", " << pose.pose.position.y << ", " << pose.pose.position.z);
  success_plan = group_pointer->plan(myplan);
  if (success_plan) 
  {
    if (wait) motion_done = group_pointer->execute(myplan);
    else motion_done = group_pointer->asyncExecute(myplan);
    if (motion_done) {
      group_pointer->setMaxVelocityScalingFactor(1.0); // Reset the velocity
      return true;
    }
  }
  ROS_WARN("Failed to perform motion.");
  group_pointer->setMaxVelocityScalingFactor(1.0); // Reset the velocity
  return false;
}


==========



// ----
int c;    

moveit_msgs::CollisionObject box;
box.header.frame_id = "b_bot_robotiq_85_tip_link";
box.id = "box";
box.primitives.resize(1);
box.primitive_poses.resize(1);
box.primitives[0].type = box.primitives[0].BOX;
box.primitives[0].dimensions.resize(3);
box.primitives[0].dimensions[0] = 0.02;
box.primitives[0].dimensions[1] = 0.05;
box.primitives[0].dimensions[2] = 0.1;
box.named_frames.resize(3);
box.frame_names.resize(3);
box.named_frames[0].position.z = -.05;
box.named_frames[0].orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -(90.0/180.0 *M_PI));
box.frame_names[0] = "box_bottom";
box.named_frames[1].position.x = -.01;
box.named_frames[1].position.y = -.025;
box.named_frames[1].position.z = -.05;
box.named_frames[1].orientation.w = 1.0;
box.frame_names[1] = "box_corner_1";
box.named_frames[2].position.x = -.01;
box.named_frames[2].position.y = .025;
box.named_frames[2].position.z = -.05;
box.named_frames[2].orientation.w = 1.0;
box.frame_names[2] = "box_corner_2";

moveit_msgs::CollisionObject cylinder;
cylinder.header.frame_id = "b_bot_robotiq_85_tip_link";
cylinder.id = "cylinder";
cylinder.primitives.resize(1);
cylinder.primitive_poses.resize(1);
cylinder.primitives[0].type = box.primitives[0].CYLINDER;
cylinder.primitives[0].dimensions.resize(2);
cylinder.primitives[0].dimensions[0] = 0.04; // height (along x)
cylinder.primitives[0].dimensions[1] = 0.005; // radius
cylinder.primitive_poses[0].position.x = 0.0;
cylinder.primitive_poses[0].position.y = -0.05;
cylinder.primitive_poses[0].position.z = 0.0;
cylinder.named_frames.resize(1);
cylinder.frame_names.resize(1);
cylinder.named_frames[0].position.y = -.05;
cylinder.named_frames[0].position.z = -.02;
box.named_frames[0].orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -(90.0/180.0 *M_PI));
cylinder.frame_names[0] = "cylinder_tip";

geometry_msgs::PoseStamped ps, ps_0;
ps = makePoseStamped();
ps.header.frame_id = "b_bot_base";
ps.pose.position.y = -.5;
ps.pose.position.z = .3;
ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, (90.0/180.0 *M_PI), 0);
// ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, (90.0/180.0 *M_PI), 0);
ps_0 = makePoseStamped();

ROS_INFO("Press key to start test stuff. \n0 to exit. \n1 to spawn box and cylinder. \n11 to spawn another thing. "
    "\n2 to attach box to the gripper \n22 to attach cylinder to the gripper "
    "\n222 to attach box to the gripper while specifying all its data again \n3 to reset scene "
    "\n4 to move to example point with EE \n5 with box \n6 with box_bottom \n7 with cylinder. "
    "\n8 with cylinder_tip \n9 to move cylinder tip to box bottom \n10 to move home. "
    "\n101-104 to move gripper to: 101 box center, 102 cylinder center, 103 box bottom, 104 cylinder tip");


  while (ros::ok())
  {
    std::cin >> c;
    if (c == 0)
    {
      return true;
    }
    if (c == 1)
    {
      ROS_INFO_STREAM("Spawning test box and cylinder.");
      moveit_msgs::CollisionObject co;
      co = box;
      co.operation = moveit_msgs::CollisionObject::ADD;
      planning_scene_interface_.applyCollisionObject(co);
      co = cylinder;
      co.operation = moveit_msgs::CollisionObject::ADD;
      planning_scene_interface_.applyCollisionObject(co);
    }
    if (c == 11)
    {
      ROS_INFO_STREAM("Spawning another test cylinder.");
      moveit_msgs::CollisionObject co;
      co = cylinder;
      co.id = "test_obj";
      co.primitive_poses[0].position.x = .05;
      co.operation = moveit_msgs::CollisionObject::ADD;
      planning_scene_interface_.applyCollisionObject(co);
    }
    if (c == 2)
    {
      moveit_msgs::AttachedCollisionObject att_coll_object;
      att_coll_object.object.id = "box";
      att_coll_object.link_name = "b_bot_robotiq_85_tip_link";
      att_coll_object.object.operation = att_coll_object.object.ADD;
      ROS_INFO_STREAM("Attaching test box.");
      planning_scene_interface_.applyAttachedCollisionObject(att_coll_object);
    }
    if (c == 22)
    {
      moveit_msgs::AttachedCollisionObject att_coll_object;
      att_coll_object.object.id = "cylinder";
      att_coll_object.link_name = "b_bot_robotiq_85_tip_link";
      att_coll_object.object.operation = att_coll_object.object.ADD;
      ROS_INFO_STREAM("Attaching cylinder.");
      planning_scene_interface_.applyAttachedCollisionObject(att_coll_object);
    }
    if (c == 222)
    {
      moveit_msgs::AttachedCollisionObject att_coll_object;
      att_coll_object.object = box;
      att_coll_object.object.id = "box";
      att_coll_object.link_name = "b_bot_robotiq_85_tip_link";
      att_coll_object.object.operation = att_coll_object.object.ADD;
      ROS_INFO_STREAM("Attaching test box WITH NAMED FRAMES.");
      planning_scene_interface_.applyAttachedCollisionObject(att_coll_object);
    }
    if (c == 3)
    {
      ROS_INFO_STREAM("Removing box and cylinder.");
      moveit_msgs::AttachedCollisionObject att_coll_object;
      att_coll_object.object = box;
      att_coll_object.link_name = "b_bot_robotiq_85_tip_link";
      att_coll_object.object.operation = att_coll_object.object.REMOVE;
      try {planning_scene_interface_.applyAttachedCollisionObject(att_coll_object);}
      catch (std::exception exc) {;}
      att_coll_object.object = cylinder;
      att_coll_object.link_name = "b_bot_robotiq_85_tip_link";
      att_coll_object.object.operation = att_coll_object.object.REMOVE;
      try {planning_scene_interface_.applyAttachedCollisionObject(att_coll_object);}
      catch (std::exception exc) {;}

      moveit_msgs::CollisionObject co;
      co = box;
      co.operation = moveit_msgs::CollisionObject::REMOVE;
      try {planning_scene_interface_.applyCollisionObject(co);}
      catch (std::exception exc) {;}
      co = cylinder;
      co.operation = moveit_msgs::CollisionObject::REMOVE;
      try {planning_scene_interface_.applyCollisionObject(co);}
      catch (std::exception exc) {;}
    }
    if (c == 4)
    {
      moveToCartPose(ps, "b_bot", true, "", 1.0);
    }
    if (c == 5)
    {
      moveToCartPose(ps, "b_bot", true, "box", 1.0);
    }
    if (c == 6)
    {
      moveToCartPose(ps, "b_bot", true, "box_bottom", 1.0);
    }
    if (c == 7)
    {
      moveToCartPose(ps, "b_bot", true, "cylinder", 1.0);
    }
    if (c == 8)
    {
      moveToCartPose(ps, "b_bot", true, "cylinder_tip", 1.0);
    }
    if (c == 9)
    {
      ps_0.header.frame_id = "box_bottom";
      ps_0.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, (180.0/180.0 *M_PI), 0);
      ps_0.pose.position.z = -0.01;
      publishMarker(ps_0, "pose");
      moveToCartPose(ps_0, "b_bot", true, "cylinder_tip", 1.0);
      ps_0.header.frame_id = "b_bot_base";
    }
    if (c == 10)
    {
      goToNamedPose("home", "b_bot");
    }
    if (c == 101)
    {
      ps_0.header.frame_id = "box";
      publishMarker(ps_0, "pose");
      moveToCartPose(ps_0, "b_bot", true, "", 1.0);
      ps_0.header.frame_id = "b_bot_base";
    }
    if (c == 102)
    {
      ps_0.header.frame_id = "cylinder";
      publishMarker(ps_0, "pose");
      moveToCartPose(ps_0, "b_bot", true, "", 1.0);
      ps_0.header.frame_id = "b_bot_base";
    }
    if (c == 103)
    {
      ps_0.header.frame_id = "cylinder_tip";
      publishMarker(ps_0, "pose");
      moveToCartPose(ps_0, "b_bot", true, "", 1.0);
      ps_0.header.frame_id = "b_bot_base";
    }
    if (c == 104)
    {
      ps_0.header.frame_id = "screw_tool_m4_tip";
      publishMarker(ps_0, "pose");
      moveToCartPose(ps_0, "b_bot", true, "", 1.0);
      ps_0.header.frame_id = "b_bot_base";
    }
    ros::spinOnce();
  }