action.hpp

Go to the documentation of this file.

/*
 * Copyright 2016 SoftBank Robotics Europe
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#ifndef ACTION_HPP
#define ACTION_HPP

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

// MoveIt!
#include <moveit/move_group_interface/move_group.h>

// Grasp generation
#include <moveit_simple_grasps/simple_grasps.h>
#include <moveit_simple_grasps/grasp_data.h>

//for showing grasps
#include <moveit_visual_tools/moveit_visual_tools.h>

// Forward kinematics to have final pose of trajectory
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>

#include "romeo_moveit_actions/metablock.hpp"
#include "romeo_moveit_actions/postures.hpp"

#define FLAG_NO_MOVE        1
#define FLAG_MOVE           2

namespace moveit_simple_actions
{

class Action
{
public:
  Action(ros::NodeHandle *nh,
         const std::string &arm,
         const std::string &robot_name,
         const std::string &base_frame);

  void initVisualTools(moveit_visual_tools::MoveItVisualToolsPtr &visual_tools);

  bool pickAction(MetaBlock *block,
                  const std::string surface_name,
                  const int attempts_nbr=0,
                  const double planning_time=0.0);

  bool placeAction(MetaBlock *block,
                   const std::string surface_name);

  bool pickDefault(MetaBlock *block,
                   const std::string surface_name);

  bool graspPlan(MetaBlock *block,
                 const std::string surface_name);

  bool graspPlanAllPossible(MetaBlock *block,
                            const std::string surface_name);

  bool executeAction();

  bool reachGrasp(MetaBlock *block,
                  const std::string surface_name,
                  int attempts_nbr=0,
                  float tolerance_min=0.0f,
                  double planning_time=0.0);

  bool reachAction(geometry_msgs::PoseStamped pose_target,
                   const std::string surface_name="",
                   const int attempts_nbr=1);

  bool poseHand(const int pose_id);

  geometry_msgs::Pose getPose();

  void poseHandOpen();

  void poseHandClose();

  bool poseHeadDown();

  bool poseHeadZero();

  void setTolerance(const double value);

  void detachObject(const std::string &block_name);

  std::string getBaseLink();

  float computeDistance(MetaBlock *block);

  float computeDistance(geometry_msgs::Pose goal);

  geometry_msgs::PoseStamped getGraspPose(MetaBlock *block);

  void releaseObject(MetaBlock *block);

  const std::string plan_group_;

private:
  void attachObject(const std::string &block_name);

  void publishPlanInfo(moveit::planning_interface::MoveGroup::Plan plan,
                       geometry_msgs::Pose pose_target);

  void setPlanningTime(const double value);

  void setToleranceStep(const double value);

  void setToleranceMin(const double value);

  void setMaxVelocityScalingFactor(const double value);

  void setVerbose(bool verbose);

  void setAttemptsMax(int value);

  void setFlag(int flag);

  std::vector<moveit_msgs::Grasp> generateGrasps(MetaBlock *block);

  std::vector<geometry_msgs::Pose> configureForPlanning(
      const std::vector<moveit_msgs::Grasp> &grasps);

  bool setAllowedMoveItCollisionMatrix(moveit_msgs::AllowedCollisionMatrix& m);

  bool getCurrentMoveItAllowedCollisionMatrix(moveit_msgs::AllowedCollisionMatrix& matrix);

  std::vector<std::string>::iterator ensureExistsInACM(const std::string& name,
                                                       moveit_msgs::AllowedCollisionMatrix& m,
                                                       bool initFlag);

  void expandMoveItCollisionMatrix(const std::string& name,
                                   moveit_msgs::AllowedCollisionMatrix& m,
                                   bool default_val);

  void updateCollisionMatrix(const std::string& name);


  const std::string end_eff_;

  Posture posture_;

  moveit_simple_grasps::GraspData grasp_data_;

  boost::scoped_ptr<move_group_interface::MoveGroup> move_group_;

  moveit_simple_grasps::SimpleGraspsPtr simple_grasps_;

  boost::shared_ptr<moveit::planning_interface::MoveGroup::Plan> current_plan_;

  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;

  moveit::planning_interface::PlanningSceneInterface current_scene_;

  ros::Publisher pub_obj_pose_;

  ros::Publisher pub_plan_pose_;

  ros::Publisher pub_plan_traj_;

  ros::ServiceClient client_fk_;

  bool verbose_;

  int attempts_max_;

  double planning_time_;

  std::string planner_id_;

  double tolerance_min_;

  double tolerance_step_;

  double max_velocity_scaling_factor_;

  float dist_th_;

  int flag_;

  std::string object_attached_;

  tf::TransformListener listener_;

  ros::Publisher planning_scene_publisher_;

  ros::ServiceClient planning_scene_client_;

  std::vector<std::string> allowedCollisionLinks_;

  std::string base_frame_;
};
}
#endif // ACTION_HPP