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(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 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
↧