Ask Your Question
0

Moving a box using tf

asked 2015-03-30 02:54:42 -0500

bluefish gravatar image

updated 2015-05-07 04:28:40 -0500

dornhege gravatar image

Hi to all,

I try to move a box in my planning scene using a tf-frame. But it's not working at the moment.

I insert the box as shown in the moveit-tutorial and giving it a tf-frame additionally like this:

 /* The id of the object is used to identify it. */
  co_box.id = "box1";
  co_box.header.frame_id = "//box_frame";

If I now insert the box, it is correctly inserted at the position and orientation I publish in my tf-broadcaster. But when I move the frame in the broadcaster, I can see the frame moving in Rviz but not the box.

What can I do here. Is the box not correctly linked to the tf-frame or do I constantly need to update the planning scene. I'm really stuck here. I would really appreciate if somebody could help me out.

Edit (the code for inserting the box and trying to move):

 // insert BOX
  // ^^^^^^^^^^

  moveit_msgs::CollisionObject co_box;
  co_box.header.frame_id = "//box_frame";

  /* The id of the object is used to identify it. */
  co_box.id = "box1";
  // co_box.header.frame_id = "box_frame";

  /* Define a box to add to the world. */
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 1.0;
  primitive.dimensions[1] = 0.2;
  primitive.dimensions[2] = 0.4;

  /* A pose for the box (specified relative to frame_id) */
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x =  0.0;
  box_pose.position.y =  0.0;
  box_pose.position.z =  0.0;

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



  std::vector<moveit_msgs::CollisionObject> collision_objects;
  collision_objects.push_back(co_box);

  ROS_INFO("Add collision objects into the world");
  planning_scene_interface.addCollisionObjects(collision_objects);
  /* Sleep so we have time to see the object in RViz */
  sleep(10.0);

  while(ros::ok()){
       co_box.operation = co_box.MOVE;
       sleeprate.sleep();
  }
edit retag flag offensive close merge delete

2 Answers

Sort by ┬╗ oldest newest most voted
0

answered 2015-05-26 05:34:27 -0500

bluefish gravatar image

Hi!

Finally I managed to solve this issue. If anybody is facing the same problem, here is my approach, which works for me:

ros::Publisher collision_object_publisher = node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 1);
  while(collision_object_publisher.getNumSubscribers() < 1)
  {
      ros::WallDuration sleep_t(0.5);
      sleep_t.sleep();
  }

  moveit_msgs::CollisionObject co;
  co.header.frame_id = "box_frame";
  co.id = "box";
  geometry_msgs::Pose pose;
  // pose.orientation.w = 1.0;
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 0.1;
  primitive.dimensions[1] = 0.1;
  primitive.dimensions[2] = 0.1;
  co.primitives.push_back(primitive);
  co.primitive_poses.push_back(pose);
  co.operation = co.ADD;
  collision_object_publisher.publish(co);


  sleep(5);


  moveit_msgs::CollisionObject move_object;
  move_object.id = "box";
  move_object.header.frame_id = "box_frame";
  move_object.primitive_poses.push_back(pose);
  move_object.operation = move_object.MOVE;

  ros::Rate loop_rate(100);

  while(ros::ok()){
    collision_object_publisher.publish(move_object);
    loop_rate.sleep();
    ROS_INFO("Another loop");
  }

Thanks @dornhege for the hints!!

edit flag offensive delete link more
1

answered 2015-05-06 02:54:40 -0500

dornhege gravatar image

If you put it into the planning scene, it's stored there in the planning frame for planning, so if it is moved, you need to update it. Use the MOVE operation in CollisionObject to only supply a new pose without resending geometry all the time.

edit flag offensive delete link more

Comments

Hi! Thanks for your answer! I tried co_box.operation = co_box.MOVE; constantly called in a while-loop but it doesn't seem to work. Do have to add an planning_scene_interface-command? Something like planning_scene.update? But I can't find an appropriate one...

bluefish gravatar imagebluefish ( 2015-05-07 01:26:08 -0500 )edit

What are you sending?

dornhege gravatar imagedornhege ( 2015-05-07 02:39:37 -0500 )edit

as I wrote above: I send co_box.operation = co_box.MOVE;

bluefish gravatar imagebluefish ( 2015-05-07 02:59:12 -0500 )edit

Or do you mean how I send the whole collision_object to the planning scene? This I do pretty much exactly as explained in the moveit c++-tutorial.

bluefish gravatar imagebluefish ( 2015-05-07 03:01:39 -0500 )edit

There are two ways: As part of a planning scene update or directly as an object. I have only tried the planning scene update, although I assume that both should have the same functionality.

dornhege gravatar imagedornhege ( 2015-05-07 03:29:22 -0500 )edit

I updated the question with the complete code for inserting the box and the try to move it. Could you tell what code excatly I have to add? (What was your code for updating the planning scene?)

bluefish gravatar imagebluefish ( 2015-05-07 03:41:29 -0500 )edit

I haven't worked with the planning scene interface. I send these directly as a planning scene message. However, I believe it should work. I have also used the MOVE operation in there.

dornhege gravatar imagedornhege ( 2015-05-07 04:28:11 -0500 )edit

Hmm. But I tried it and cannot see any movement... As before the tf-frame is moving but not the object... :(

bluefish gravatar imagebluefish ( 2015-05-07 06:25:20 -0500 )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: 2015-03-30 02:54:42 -0500

Seen: 1,022 times

Last updated: May 26 '15