Adding Collision Object in MoveIt

edit

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= 1.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?