How to build a work scene with a unique or complex CAD(stl) model in moveit environment?
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!