Ask Your Question
1

Adding Collision Object in MoveIt

asked 2016-10-19 07:45:34 -0600

bhavyadoshi26 gravatar image

updated 2016-10-25 09:13:49 -0600

Hi,

I have written a node to add a wall as a collision object in moveit.

This is my node :

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

using namespace Eigen; 

int main(int argc, char **argv)
{
    ros::init(argc, argv, "add_wall");
    ros::NodeHandle nh;
    ros::AsyncSpinner spin(1);
    spin.start();

    moveit::planning_interface::PlanningSceneInterface current_scene;
    sleep(2.0);

    Vector3d b(0.001, 0.001, 0.001);
    moveit_msgs::CollisionObject co;
    co.id = "wall";
    shapes::Mesh* m = shapes::createMeshFromResource("package://mitsubishi_rv6sd_support/meshes/wall.stl",b); 
    ROS_INFO("Wall mesh loaded");

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

    co.meshes.resize(1);
    co.mesh_poses.resize(1);
    co.meshes[0] = mesh;
    co.header.frame_id = "wall";   
    co.mesh_poses[0].position.x = 0.560651;
    co.mesh_poses[0].position.y = 0.579113;
    co.mesh_poses[0].position.z = 0.0;
    co.mesh_poses[0].orientation.w= 0.0; 
    co.mesh_poses[0].orientation.x= 0.0; 
    co.mesh_poses[0].orientation.y= 0.0;
    co.mesh_poses[0].orientation.z= 0.0;   

    co.meshes.push_back(mesh);
    co.mesh_poses.push_back(co.mesh_poses[0]);
    co.operation = co.ADD;
    std::vector<moveit_msgs::CollisionObject> vec;
    vec.push_back(co);
    ROS_INFO("Wall added into the world");
    current_scene.addCollisionObjects(vec);
    sleep(5.0);
    ros::shutdown();
    return 0;
}

When I run the node after running demo.launch, I am able to see the wall along with my robot in RViZ. But I get the following error :

[ INFO] [1476879844.724728125]: Wall mesh loaded
[ INFO] [1476879844.724948036]: Wall added into the world

[ERROR] [1476879844.947753744]: Unable to transform from frame 'wall' to frame '/base_link'. Returning identity.

[add_wall-10] process has finished cleanly
log file: /home/sbr/.ros/log/eaf6e28a-95f6-11e6-9e7f-480fcf447f3c/add_wall-10*.log

I was trying to add this as a static_transform_publisher in the demo.launch file

<node pkg="tf" type="static_transform_publisher" name="base_link_to_wall" args=" 0.560651 0.579113 0 0 0 0 base_link wall 100" />

But this doesn't seem to be the right solution.

Any suggestions on how I can proceed?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-10-19 08:19:04 -0600

rbbg gravatar image

Hi,

If you have a look at this tutorial, you will find that in the header you don't want to put the id of the object, but the reference frame. This will probably be something like "world" but it depends on your configuration.

edit flag offensive delete link more

Comments

I have a /base_link frame which is the robot's frame. I am using that as reference for collision objects. Following the moveit tutorials, I have created a virtual joint called world_joint between the world and base_link frame. I am changing the header to /base_link. Is that alright?

bhavyadoshi26 gravatar imagebhavyadoshi26 ( 2016-10-19 09:45:54 -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

1 follower

Stats

Asked: 2016-10-19 07:45:34 -0600

Seen: 4,057 times

Last updated: Oct 25 '16