Ask Your Question
0

How to build a work scene with a unique or complex CAD(stl) model in moveit environment?

asked 2018-05-22 22:26:32 -0500

Hanker_SIA gravatar image

updated 2018-07-05 22:36:25 -0500

jayess gravatar image

As above description, i want build a work scene using a CAD model designed by myself and i had the related urdf file, while i can only find the method using moveit_msgs::CollisionObject, shape_msgs::SolidPrimitive class etc to describe a simple shape and handle it in the application node such as kinoa arm demo node pick_place.cpp. So how can i load a complex cad model not just box or cylinder and so on described in urdf and stl file and use that model to build my work scene like using shape_msgs::SolidPrimitive class as following:

void PickPlace::build_workscene()
{
    co_.header.frame_id = "root";
    co_.header.stamp = ros::Time::now();

    // remove table
    co_.id = "table";
    co_.operation = moveit_msgs::CollisionObject::REMOVE;
    pub_co_.publish(co_);

    // add table
    co_.primitives.resize(1);
    co_.primitive_poses.resize(1);
    co_.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
     co_.primitives[0].dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
    co_.operation = moveit_msgs::CollisionObject::ADD;

    co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 2.4;
    co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 2.4;
    co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.03;
    co_.primitive_poses[0].position.x = 0;
    co_.primitive_poses[0].position.y = 0.0;
    co_.primitive_poses[0].position.z = -0.03/2.0;
    pub_co_.publish(co_);
    planning_scene_msg_.world.collision_objects.push_back(co_);
    planning_scene_msg_.is_diff = true;
    pub_planning_scene_diff_.publish(planning_scene_msg_);
    ros::WallDuration(0.1).sleep();
}

I had tried to add the target urdf section of target model to the xacro file using for moveit config, and i can see the target model in Rviz. But i did not get a tutorial to guide how to get the model id and use it in the application node. Any clues will be thankful! Please!

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2018-06-28 02:20:26 -0500

Hanker_SIA gravatar image

updated 2018-06-28 02:22:42 -0500

Luckily, I have found a solution! And the add_meshModel(const char name,const char path, geometry_msgs::Pose prePose) function is key code snippet. The method is as follows:

class build_workScene
{
public:
    build_workScene(ros::NodeHandle &nh);
    void add_boxModel(const char *name,float length, float width, float height, geometry_msgs::Pose prePose);
    void add_meshModel(const char *name,const char* path, geometry_msgs::Pose prePose);
    void clear_WorkScene(const char *objName);

private:
    /* data */
    ros::NodeHandle nh_;
    ros::Publisher pub ;
    ros::Publisher pub_aco_;
    ros::Publisher pub_planning_scene_diff_;

    moveit_msgs::CollisionObject co_;
    geometry_msgs::PoseStamped can_pose_;
    //work scene
    moveit_msgs::AttachedCollisionObject aco_;
    moveit_msgs::PlanningScene planning_scene_msg_;
};

build_workScene::build_workScene(ros::NodeHandle &nh):nh_(nh)
{
    pub = nh_.advertise<moveit_msgs::CollisionObject>("/collision_object", 1);
    pub_aco_ = nh_.advertise<moveit_msgs::AttachedCollisionObject>("/attached_collision_object", 10);
    pub_planning_scene_diff_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
    ros::Duration(1.0).sleep();
}

void build_workScene::add_boxModel(const char *name,float length, float width, float height, geometry_msgs::Pose prePose)
{
    co_.header.frame_id = "root";
    co_.header.stamp = ros::Time::now();

    co_.id = name;
    co_.operation = moveit_msgs::CollisionObject::REMOVE;
    pub.publish(co_);

    // add table
    co_.primitives.resize(1);
    co_.primitive_poses.resize(1);
    co_.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
    co_.primitives[0].dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
    co_.operation = moveit_msgs::CollisionObject::ADD;

    co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = length;//table:2.4*2.4*0.03
    co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = width;
    co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = height;
    co_.primitive_poses[0].position.x =prePose.position.x;//x=0,y=0,z=-0.03/2.0
    co_.primitive_poses[0].position.y =prePose.position.y; 0.0;
    co_.primitive_poses[0].position.z =prePose.position.z;

    co_.primitive_poses[0].orientation.x =prePose.orientation.x;
    co_.primitive_poses[0].orientation.y =prePose.orientation.y;
    co_.primitive_poses[0].orientation.z =prePose.orientation.z;
    co_.primitive_poses[0].orientation.w =prePose.orientation.w;
    pub.publish(co_);
    planning_scene_msg_.world.collision_objects.push_back(co_);
    planning_scene_msg_.is_diff = true;
    pub_planning_scene_diff_.publish(planning_scene_msg_);
    ros::WallDuration(0.1).sleep();    
}

void build_workScene::add_meshModel(const char *name,const char* path, geometry_msgs::Pose prePose)
{  
    co_.id = name;
    co_.operation = moveit_msgs::CollisionObject::REMOVE;
    pub.publish(co_);

    //add hollowPrism
    co_.primitives.resize(1);
    co_.primitive_poses.resize(1);
    co_.operation = moveit_msgs::CollisionObject::ADD;

    shapes::Mesh* hollowPrism_shape = shapes::createMeshFromResource(path);
    shapes::ShapeMsg hollowPrism_mesh_msg;
    shapes::constructMsgFromShape(hollowPrism_shape, hollowPrism_mesh_msg);
    shape_msgs::Mesh hollowPrism_mesh = boost::get<shape_msgs::Mesh>(hollowPrism_mesh_msg);

    co_.meshes.push_back(hollowPrism_mesh);
    co_.mesh_poses.push_back(prePose);
    can_pose_.pose.position.x = co_.primitive_poses[0].position.x;
    can_pose_.pose.position.y = co_.primitive_poses[0].position.y;
    can_pose_.pose.position.z = co_.primitive_poses[0].position.z;

    pub.publish(co_);
    planning_scene_msg_.world.collision_objects.push_back(co_);
    planning_scene_msg_.is_diff = true;
    pub_planning_scene_diff_.publish(planning_scene_msg_); 
}

void build_workScene::clear_WorkScene(const char *objName)
{
    co_.id = objName;
    co_.operation = moveit_msgs::CollisionObject::REMOVE;
    pub.publish(co_);

    planning_scene_msg_.world.collision_objects.push_back(co_);
    planning_scene_msg_.is_diff = true;
    pub_planning_scene_diff_.publish(planning_scene_msg_);
    ros::WallDuration(0.1).sleep();
}
edit flag offensive delete link more

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: 2018-05-22 22:26:32 -0500

Seen: 313 times

Last updated: Jul 05 '18