Ask Your Question
0

collisionobject/mesh not added to rviz scene from stl file

asked 2018-12-12 09:11:38 -0500

DieterWuytens gravatar image

updated 2018-12-13 00:56:10 -0500

gvdhoorn gravatar image

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-12-13 00:52:03 -0500

Pulkit123 gravatar image

updated 2019-01-14 00:28:24 -0500

jayess gravatar image

Hi just add

#include "geometric_shapes/mesh_operations.h"

in your code and try with these lines of code :

shapes::Mesh* c_mesh = shapes::createMeshFromResource("package://robot_config/wall.stl", b); shapes::ShapeMsg mesh_msg; 
shapes::constructMsgFromShape(c_mesh, mesh_msg); 
shape_msgs::Mesh custom_mesh = boost::get<shape_msgs::mesh>(mesh_msg);
edit flag offensive delete link more

Comments

I found the solution in:

https://answers.ros.org/question/2343...

Thanks!

DieterWuytens gravatar image DieterWuytens  ( 2018-12-13 13:38:20 -0500 )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: 2018-12-12 09:11:38 -0500

Seen: 277 times

Last updated: Jan 14 '19