0
$\begingroup$

Rosanswers logo

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();
  }

Originally posted by bluefish on ROS Answers with karma: 236 on 2015-03-30

Post score: 0

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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.


Originally posted by dornhege with karma: 31395 on 2015-05-06

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by bluefish on 2015-05-07:
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...

Comment by dornhege on 2015-05-07:
What are you sending?

Comment by bluefish on 2015-05-07:
as I wrote above: I send co_box.operation = co_box.MOVE;

Comment by bluefish on 2015-05-07:
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.

Comment by dornhege on 2015-05-07:
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.

Comment by bluefish on 2015-05-07:
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?)

Comment by dornhege on 2015-05-07:
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.

Comment by bluefish on 2015-05-07:
Hmm. But I tried it and cannot see any movement... As before the tf-frame is moving but not the object... :(

Comment by bluefish on 2015-05-07:
@dornhege: could you maybe post the code snippet of inserting and moving objects via planning scene messages? Would really appreciate!

$\endgroup$
0
$\begingroup$

Rosanswers logo

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!!


Originally posted by bluefish with karma: 236 on 2015-05-26

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.