ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

[SOLVED] Number of primitive shapes does not match number of poses in collision object message

asked 2020-07-16 06:49:26 -0600

Quark gravatar image

updated 2020-07-28 02:03:14 -0600

Hi, i would like to add a STL collision object. I have similar code and it works perfectly with primitive box. I followed https://answers.ros.org/question/2311... but it still writes me [ERROR] [1594897964.204647300]: Number of primitive shapes does not match number of poses in collision object message And i cannot see an *.STL file in RViz. ROS Melodic, windows 10 - Ubuntu terminal, Xming.

Any adwices what i'm doing wrong? Thank you. (Apologize for bad english).

uname -a

Linux ### 4.4.0-18362-Microsoft #836-Microsoft Mon May 05 16:04:00 PST 2020 x86_64 x86_64 x86_64 GNU/Linux

float addMeshCollObj(float fX, float fY, float fZ, string sName){

    static const std::string PLANNING_GROUP = "arm";
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    const robot_state::JointModelGroup* joint_model_group =move_group.getCurrentState()>getJointModelGroup(PLANNING_GROUP);
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = sName;
    Vector3d b(0.001, 0.001, 0.001);
    shapes::Mesh* m = shapes::createMeshFromResource("package://robot_rx160/meshes/gtbx/Ramecek.STL",b);

    shape_msgs::Mesh mesh;
    shapes::ShapeMsg mesh_msg;  
    shapes::constructMsgFromShape(m, mesh_msg);
    mesh = boost::get<shape_msgs::Mesh>(mesh_msg);

    collision_object.meshes.resize(1);
    collision_object.mesh_poses.resize(1);
    collision_object.meshes[0] = mesh;

    collision_object.mesh_poses[0].position.x= fX;
    collision_object.mesh_poses[0].position.y= fY;
    collision_object.mesh_poses[0].position.z= fZ;
    collision_object.mesh_poses[0].orientation.w=0.0; 
    collision_object.mesh_poses[0].orientation.x=0.0; 
    collision_object.mesh_poses[0].orientation.y=0.0;
    collision_object.mesh_poses[0].orientation.z=0.0;   

    collision_object.meshes.push_back(mesh);
    collision_object.mesh_poses.push_back(collision_object.mesh_poses[0]);
    collision_object.operation = collision_object.ADD;

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);
    ROS_INFO("Add an object \"%s\" into the world", sName.c_str());
    planning_scene_interface.addCollisionObjects(collision_objects);

    sleep(1.0);
    return 0.0;
}
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2020-07-28 02:02:55 -0600

Quark gravatar image

updated 2020-07-28 02:04:23 -0600

I just solve it. The problem was in the vector. I removed at the end the ,b from

shapes::Mesh* m = shapes::createMeshFromResource("package://robot_rx160/meshes/gtbx/Ramecek.STL",b);

so

shapes::Mesh* m = shapes::createMeshFromResource("package://robot_rx160/meshes/gtbx/Ramecek.STL");

and also removed the lines

collision_object.meshes.push_back(mesh);

collision_object.mesh_poses.push_back(collision_object.mesh_poses[0]);

And now it is OK. Thank you for help.

edit flag offensive delete link more
0

answered 2020-07-21 03:04:33 -0600

fvd gravatar image

Some comments that might not solve your problem:

  1. Why do you add the same object to the list twice via push_back? Try deleting those two lines.
  2. Have you confirmed that your mesh is read in correctly? You can use the print function or check for nullptr.
  3. [0,0,0,0] is not a valid quaternion. This is surely corrected automatically, but if you want the neutral orientation you should set orientation.w = 1.0 anyway.
edit flag offensive delete link more

Comments

1) when I delete the lines with push_back the object shows up in Object scene table in RViz but it is not visible, i can see only robot.

2) Mesh should be fine, i was using it when I was trying the same in Python, it works.

Quark gravatar image Quark  ( 2020-07-27 03:42:35 -0600 )edit

Try printing the message to your terminal before it is used by putting ROS_INFO_STREAM(collision_object); before the line with the planning_scene_interface.

fvd gravatar image fvd  ( 2020-07-27 21:45:58 -0600 )edit

The output:

seq: 0 stamp: 0.000000000 frame_id: base_link id: mesh type: key: db: primitives[] primitive_poses[] meshes[] meshes[0]: triangles[] triangles[0]: vertex_indices[] vertex_indices[0]: 0 vertex_indices[1]: 1 vertex_indices[2]: 2 triangles[1]: vertex_indices[] vertex_indices[0]: 0 vertex_indices[1]: 2 vertex_indices[2]: 3 triangles[2]: vertex_indices[] vertex_indices[0]: 3 vertex_indices[1]: 2 vertex_indices[2]: 4 triangles[3]: vertex_indices[] vertex_indices[0]: 5 vertex_indices[1]: 6 vertex_indices[2]: 2 triangles[4]: vertex_indices[] vertex_indices[0]: 2 vertex_indices[1]: 6 vertex_indices[2]: 4 triangles[5]: vertex_indices[] vertex_indices[0]: 1

Quark gravatar image Quark  ( 2020-07-28 00:22:08 -0600 )edit

That is cut off and not very helpful to have here, just look through it on your end and see if anything is missing or strange. Have you tried using applyCollisionObject instead?

fvd gravatar image fvd  ( 2020-07-28 01:35:49 -0600 )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

Stats

Asked: 2020-07-16 06:49:26 -0600

Seen: 131 times

Last updated: Jul 28 '20