1 #ifndef MOVEIT_GRASP_MEDIATOR_
2 #define MOVEIT_GRASP_MEDIATOR_
5 #include <ros/package.h>
6 #include <yaml-cpp/yaml.h>
13 #include <gb_grasp/VoxelManager.h>
14 #include <gb_grasp/GraspMap.h>
40 Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle>
const& nh);
49 bool generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose,
double& x_depth,
double& y_width,
double& z_height);
50 bool getIKSolution(
const moveit::core::JointModelGroup* arm_jmg,
const Eigen::Isometry3d& target_pose, robot_state::RobotState& solution,
const std::string& link_name);
51 bool planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
52 moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
53 moveit_msgs::MotionPlanResponse& pre_approach_plan,
const std::string& object_name);
54 void visualizePick(
const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
55 const moveit_msgs::MotionPlanResponse& pre_approach_plan);
56 void waitForNextStep(
const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools,
const std::string& prompt);
57 bool planPreApproach(
const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan);
58 bool isStateValid(
const planning_scene::PlanningScene* planning_scene,
59 const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state,
60 const robot_model::JointModelGroup* group,
const double* ik_solution);