MoveItPlanner.h

Go to the documentation of this file.

#ifndef GRASP_EXECUTION_REACH_TEST_MOVEITPLANNER_H
#define GRASP_EXECUTION_REACH_TEST_MOVEITPLANNER_H

#include <ros/ros.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit_msgs/MotionPlanResponse.h>
#include <moveit_msgs/GetMotionPlan.h>
#include <moveit_msgs/GetStateValidity.h>
#include <moveit_msgs/GetCartesianPath.h>
#include <moveit_msgs/RobotTrajectory.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/PlanningScene.h>

namespace moveit_planning_helper
{

class MoveItPlanner
{
public:
    MoveItPlanner(ros::NodeHandle& n,
                const std::string& _moveit_motion_plan_service="/plan_kinematic_path",
                const std::string& _moveit_check_state_validity_service="/check_state_validity");

    ~MoveItPlanner();


        static moveit_msgs::Constraints getPoseConstraint(const std::string& link_name,
        const geometry_msgs::PoseStamped& pose, double tolerance_pos, double tolerance_angle, int type);


        static moveit_msgs::OrientationConstraint getOrientationConstraint(const std::string& link_name,
        const geometry_msgs::QuaternionStamped& quat, 
                const float& x_tolerance, 
                const float& y_tolerance,
                const float& z_tolerance); 

        static moveit_msgs::Constraints getJointConstraint(const sensor_msgs::JointState& joint_state,
        const float& joint_tolerance);

        static moveit_msgs::PositionConstraint getSpherePoseConstraint(
                        const std::string &link_name, 
                        const geometry_msgs::PoseStamped &target_pose, 
                        float arm_reach_span);
        
        static moveit_msgs::PositionConstraint getBoxConstraint(const std::string &link_name,
        const geometry_msgs::PoseStamped& box_origin, const double& x, const double& y, const double& z);

   
    moveit_msgs::MoveItErrorCodes requestTrajectory(
        const geometry_msgs::PoseStamped& arm_base_pose,
        float arm_reach_span,
        const std::string& planning_group, 
        const moveit_msgs::Constraints& goal_constraints,
        const moveit_msgs::Constraints * path_constraints,  
        const sensor_msgs::JointState& start_state,
        moveit_msgs::RobotTrajectory& result_traj); 

    moveit_msgs::MoveItErrorCodes requestTrajectoryForMobileRobot(
        const geometry_msgs::PoseStamped& start_pose, 
        const geometry_msgs::PoseStamped& target_pose, 
        float arm_reach_span, const std::string& planning_group, 
        const moveit_msgs::Constraints& goal_constraints,
        const moveit_msgs::Constraints * path_constraints,  
        const sensor_msgs::JointState& start_state,
        const std::string& virtual_joint_name,
        const std::string& target_frame_virtual_joint,
        moveit_msgs::RobotTrajectory& result_traj); 

private:
        
    bool init();
        void shutdown();

    moveit_msgs::MoveItErrorCodes requestTrajectory(const std::string& group_name, 
        const std::vector<moveit_msgs::Constraints>& goal_constraints, 
        const sensor_msgs::JointState& from_state, 
        const moveit_msgs::WorkspaceParameters& wspace,
        const sensor_msgs::MultiDOFJointState *mdjs,
        const moveit_msgs::Constraints* path_constraints,
        moveit_msgs::RobotTrajectory& result);


    moveit_msgs::MotionPlanResponse sendServiceRequest(moveit_msgs::MotionPlanRequest& request); 

    bool makeWorkspace(const geometry_msgs::PoseStamped& from,
                       const geometry_msgs::PoseStamped& to,
                       float arm_reach_span, moveit_msgs::WorkspaceParameters& result) const;

    bool makeWorkspace(const geometry_msgs::PoseStamped& origin,
                       float arm_reach_span, 
                       moveit_msgs::WorkspaceParameters& wspace) const;



    bool isValidState(const moveit_msgs::RobotState& robot_state, const std::string& group);

        std::string moveit_motion_plan_service;
        std::string moveit_check_state_validity_service;

        ros::ServiceClient motion_plan_client;
        ros::ServiceClient state_validity_client;

    ros::NodeHandle node;


};
}  // namespace

#endif   // GRASP_EXECUTION_REACH_TEST_MOVEITPLANNER_H