ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A answers.ros.org
Ask Your Question
0

Adding the collision object "wall" to moveit-rviz

asked 2017-10-21 06:38:40 -0500

EdwinvanEmmerik gravatar image

updated 2017-10-21 08:31:13 -0500

v4hn gravatar image

Hi everyone,

I was trying to add a wall to my robot its planning scene by creating a node that will spawn this wall. With these walls I want to set the workspace.

When I run it I don't get any errors but nothing really happens either.

This is the code:

#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 = "manipulator";
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    Eigen::Vector3d b(0.001, 0.001, 0.001);

    moveit_msgs::CollisionObject collision_object;
    collision_object.id = "wall";
    shapes::Mesh* m = shapes::createMeshFromResource("package://ur_description/meshes/ur5/collision/box1.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 = -0.5;
    collision_object.mesh_poses[0].position.y = -0.5;
    collision_object.mesh_poses[0].position.z = -0.1;
    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.addCollisionObjects(collision_vector);
    ROS_INFO("Wall added into the world");
    move_group.attachObject(collision_object.id);
    sleep(5.0);
    ros::shutdown();
    return 0;
}

Would be nice if somebody could help me out!

Kind regards, Edwin van Emmerik

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2017-10-21 08:40:04 -0500

v4hn gravatar image

You definitely do not want to attach your wall to your planning group in the end. Attaching only makes sense for objects the robot grasped.

Use planning_scene_interface.applyCollisionObject instead of addCollisionObject unless you know what you are doing. This should spawn your object successfully. We should really rework the tutorial here and advertise apply* as the default option...

Notice that if your workspace is static throughout runtime, the more simple way to add walls is to model them as part of the robot in your urdf/xacro instead. In that case they also remain there after someone clears the planning scene.

edit flag offensive delete link more

Comments

Thank you very much! I do want to keep my workspace able to change for now though. Should I add it to the google group with a link to this question?

EdwinvanEmmerik gravatar imageEdwinvanEmmerik ( 2017-10-21 08:53:49 -0500 )edit

You're welcome. No need to add a link to the mailing list. Software usage questions don't belong there in the first place.

v4hn gravatar imagev4hn ( 2017-10-21 09:13:58 -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

2 followers

Stats

Asked: 2017-10-21 06:38:40 -0500

Seen: 537 times

Last updated: Oct 21 '17