moveit_grasp_mediator.h

#ifndef MOVEIT_GRASP_MEDIATOR_
#define MOVEIT_GRASP_MEDIATOR_

#include <ros/ros.h>
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <fstream>
#include "impl/abstract_mediator.h"
#include "impl/moveit_mediator.h"

#include "impl/abstract_robot.h"
#include "impl/robot.h"
#include <gb_grasp/VoxelManager.h>
#include <gb_grasp/GraspMap.h>
#include "reader/abstract_param_reader.h"
#include "reader/robot_reader.h"
#include "reader/cuboid_reader.h"
#include "reader/job_reader.h"




class Moveit_grasp_mediator : public Moveit_mediator{
    protected:
    std::shared_ptr<planning_pipeline::PlanningPipeline> planning_pipeline_;
    std::shared_ptr<VoxelManager> voxel_manager_;
    std::string grasp_ns_;
    boost::dynamic_bitset<> voxel_environment_;
    std::vector<GraspMap> grasp_maps_;


    // Readers 
    std::unique_ptr<Robot_reader> robot_reader_;
    std::unique_ptr<Abstract_param_reader> wing_reader_;
    std::unique_ptr<Abstract_param_reader> cuboid_reader_;
    std::unique_ptr<Job_reader> job_reader_;


    public:
    Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);

    
    void mediate() override;
    void connect_robots(Abstract_robot* robot) override;

    void manipulate_grasp_data(Moveit_robot* robot);
    void scene_setup();
    void manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size);
    bool generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth, double& y_width, double& z_height);
    bool getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose, robot_state::RobotState& solution, const std::string& link_name);
    bool planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
                     moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
                     moveit_msgs::MotionPlanResponse& pre_approach_plan, const std::string& object_name);
    void visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
                     const moveit_msgs::MotionPlanResponse& pre_approach_plan);
    void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt);
    bool planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan);
    bool isStateValid(const planning_scene::PlanningScene* planning_scene,
                  const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state,
                  const robot_model::JointModelGroup* group, const double* ik_solution);




};

#endif