// 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;
}
