// Execute series of tasks for pick/place bool pickAndPlace(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose) { ROS_INFO_STREAM_NAMED("pick_place","Pick and place started"); // --------------------------------------------------------------------------------------------- // Visualize the two blocks rviz_tools_->publishBlock(start_block_pose, BLOCK_SIZE, true); rviz_tools_->publishBlock(end_block_pose, BLOCK_SIZE, false); // --------------------------------------------------------------------------------------------- // Generate graps ROS_INFO_STREAM_NAMED("pick_place_moveit","Generating grasps for pick and place"); bool rviz_verbose = true; block_grasp_generator::GraspGenerator grasp_generator( base_link_, PLANNING_GROUP_NAME, rviz_tools_, rviz_verbose); // Pick grasp std::vector<manipulation_msgs::Grasp> possible_grasps; grasp_generator.generateGrasps( start_block_pose, possible_grasps ); // Filter grasp poses //block_grasp_generator::GraspFilter grasp_filter( planning_scene_monitor_->getPlanningScene()->getCurrentState() ... //if( !grasp_generator.filterGrasps( possible_grasps ) ) //return false; // Send pick command to move_group executeGrasps(possible_grasps, start_block_pose); return true; }
// Execute series of tasks for pick/place bool pickAndPlace(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose) { ROS_INFO_STREAM_NAMED("pick_place","Pick and place started"); // --------------------------------------------------------------------------------------------- // Visualize the two blocks rviz_tools_->publishBlock(start_block_pose); rviz_tools_->publishBlock(end_block_pose); // --------------------------------------------------------------------------------------------- // Generate graps ROS_INFO_STREAM_NAMED("pick_place_moveit","Generating grasps for pick and place"); bool rviz_verbose = true; moveit_simple_grasps::SimpleGrasps grasp_generator(rviz_tools_); // Pick grasp std::vector<moveit_msgs::Grasp> possible_grasps; grasp_generator.generateBlockGrasps( start_block_pose, grasp_data_, possible_grasps ); // Filter grasp poses //moveit_simple_grasps::GraspFilter grasp_filter( planning_scene_monitor_->getPlanningScene()->getCurrentState() ... //if( !grasp_generator.filterGrasps( possible_grasps ) ) //return false; // Send pick command to move_group executeGrasps(possible_grasps, start_block_pose); return true; }