#ifndef SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H
#define SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H

// ROS
#include <ros/ros.h>

// MoveIt
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>

namespace pick_place_planner {

/**
 * Sets the posture argument to an open state for the two gripper fingers (8cm apart).
 * @param posture An empty posture that is filled with a posture for two fingers
 */
void openGripper(trajectory_msgs::JointTrajectory &posture);

/**
 * Sets the posture argument to a closed state for the two gripper fingers
 * @param posture An empty posture that is filled with a posture for two fingers
 */
void closedGripper(trajectory_msgs::JointTrajectory &posture);

/**
 * Pick an object from a fixed position.
 * @param move_group The move group the pick operation is performed on
 */
moveit::planning_interface::MoveItErrorCode pick(moveit::planning_interface::MoveGroupInterface &move_group);

/**
* Place an object at a fixed position on top of table2.
* @param group
*/
moveit::planning_interface::MoveItErrorCode place(moveit::planning_interface::MoveGroupInterface &group);

/**
* Add the collision objects to the planning scene and to gazebo.
* @param planning_scene_interface The interface to access the planning scene.
*/
void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface &planning_scene_interface);

}

#endif // SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H