Hi,
I have written a node to add a wall as a collision object in moveit.
This is my node :
#include
#include
#include
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(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 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
↧