collisionobject/mesh not added to rviz scene from stl file
Hello, i am currently trying to add a .stl file to my rviz scene with following script: I want to add a wall.stl file that i made for collision detection. I'm using ros kinetic.
#include "ros/ros.h"
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_scene_interface/planning_scene_interface.h"
#include "geometric_shapes/shape_operations.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "building_workspace");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
const std::string PLANNING_GROUP = "endeffector";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
sleep(2.0);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
sleep(2.0);
Eigen::Vector3d b(0.001, 0.001, 0.001);
sleep(2.0);
moveit_msgs::CollisionObject collision_object;
collision_object.id = "wall";
shapes::Mesh* m = shapes::createMeshFromResource("package://robot_config/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);
collision_object.meshes.resize(1);
collision_object.mesh_poses.resize(1);
collision_object.meshes[0] = mesh;
collision_object.header.frame_id = move_group.getPlanningFrame();
collision_object.mesh_poses[0].position.x = 1.0;
collision_object.mesh_poses[0].position.y = 1.0;
collision_object.mesh_poses[0].position.z = 0.0;
collision_object.mesh_poses[0].orientation.x = 1.57;
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_vector;
collision_vector.push_back(collision_object);
planning_scene_interface.applyCollisionObjects(collision_vector);
ROS_INFO("Wall added into the world");
move_group.attachObject(collision_object.id);
sleep(5.0);
ros::shutdown();
return 0;
In the table Scene Object in RVIZ i'm seeing "wall" but it's not added to the scene of my robot. The planningroup my robot is using is endeffector.
What is my mistake?
Thanks in advance.