simplepickplace.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 SIMPLEACTIONS_HPP
#define SIMPLEACTIONS_HPP

#include <ros/ros.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include "romeo_moveit_actions/metablock.hpp"
#include "romeo_moveit_actions/action.hpp"
#include "romeo_moveit_actions/objprocessing.hpp"
#include "romeo_moveit_actions/evaluation.hpp"

namespace moveit_simple_actions
{

class SimplePickPlace
{
public:
  SimplePickPlace(const std::string robot_name,
                  const double test_step,
                  const double x_min,
                  const double x_max,
                  const double y_min,
                  const double y_max,
                  const double z_min,
                  const double z_max,
                  const std::string left_arm_name,
                  const std::string right_arm_name,
                  const bool verbose);

  void run();

protected:
  MetaBlock createTable();

  void switchArm(Action *action_now);

  void createObj(const MetaBlock &block);

  void resetBlock(MetaBlock *block);

  void getCollisionObjects(const moveit_msgs::CollisionObject::ConstPtr& msg);

  void cleanObjects(std::vector<MetaBlock> *objects,
                    const bool list_erase=true);

  bool checkObj(int &block_id);

  ros::NodeHandle nh_, nh_priv_;

  std::string robot_name_;

  const bool verbose_;

  std::string base_frame_;

  double block_size_x_;

  double block_size_y_;

  double floor_to_base_height_;

  ObjProcessor obj_proc_;

  Evaluation evaluation_;

  bool env_shown_;

  //the working space of the robot
  double x_min_;
  double x_max_;
  double y_min_;
  double y_max_;
  double z_min_;
  double z_max_;

  std::string support_surface_;

  Action *action_left_;

  Action *action_right_;

  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;

  moveit::planning_interface::PlanningSceneInterface current_scene_;

  std::vector<MetaBlock> blocks_surfaces_;

  ros::Subscriber sub_obj_coll_;

  ros::Publisher pub_obj_pose_;

  geometry_msgs::PoseStamped msg_obj_pose_;

  geometry_msgs::Pose pose_default_;

  geometry_msgs::Pose pose_default_r_;

  geometry_msgs::Pose pose_zero_;

  std::vector <geometry_msgs::Pose> stat_poses_success_;

  ros::Publisher pub_obj_moveit_;

  ros::Rate rate_;
};
}

#endif // SIMPLEACTIONS_HPP