interactive_manipulation_frame.h

Go to the documentation of this file.

/*
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef INTERACTIVE_MANIPULATION_FRAME
#define INTERACTIVE_MANIPULATION_FRAME

#include "pr2_interactive_manipulation/interactive_manipulation_ui.h"
#include "pr2_interactive_manipulation/object_selection_frame.h"
#include "pr2_interactive_manipulation/advanced_options_dialog.h"
#include "pr2_interactive_manipulation/gripper_controller.h"

#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <ros/ros.h>

#include <object_manipulator/tools/mechanism_interface.h>

#include <tabletop_collision_map_processing/collision_map_interface.h>

#include <object_manipulation_msgs/PickupAction.h>
#include <object_manipulation_msgs/PlaceAction.h>

#include <pr2_create_object_model/ModelObjectInHandAction.h>
#include <pr2_create_object_model/ObjectInHand.h>

#include <pr2_interactive_object_detection/GraspableObjectList.h>

#include <sensor_msgs/point_cloud_conversion.h>

namespace pr2_interactive_manipulation {

class InteractiveManipulationDisplay;

class InteractiveManipulationFrame : public InteractiveManipulationFrameBase
{
public:
  InteractiveManipulationFrame(InteractiveManipulationDisplay *display, wxWindow *parent, 
                               rviz::VisualizationManager* manager);

  ~InteractiveManipulationFrame();

  void update();

  void setAdvancedGraspOptions(AdvancedGraspOptions ago){ago_ = ago;}

protected:

  struct GraspInfo
  {
    static geometry_msgs::Pose somePose()
    {
      geometry_msgs::Pose pose;
      pose.position.x = -0.08;
      pose.position.y = -0.00;
      pose.position.z = 0.27; 
      pose.orientation.x = -0.517361;
      pose.orientation.y =  0.417646;
      pose.orientation.z =  0.323392;
      pose.orientation.w =  0.673295; 
      return pose;
    }

    GraspInfo()
    {
      //some random grasp pose, so at least we don't get nans and infs
      grasp_.grasp_pose = somePose();
    }
    std::string object_collision_name_;
    object_manipulation_msgs::Grasp grasp_;
    sensor_msgs::PointCloud2 object_cluster_;
  };

  struct DialogOptions
  {
    bool collision_checked_;
    int arm_selection_;
  };

  virtual void graspButtonClicked( wxCommandEvent& event );

  virtual void placeButtonClicked( wxCommandEvent& event );

  virtual void cancelButtonClicked( wxCommandEvent& event );

  virtual void takeMapButtonClicked( wxCommandEvent& event );

  virtual void resetButtonClicked( wxCommandEvent& event );

  virtual void armGoButtonClicked( wxCommandEvent& event );

  virtual void lookButtonClicked( wxCommandEvent& event );

  virtual void collisionBoxClicked( wxCommandEvent& event );

  virtual void advancedOptionsClicked( wxCommandEvent& event );

  virtual void modelObjectClicked( wxCommandEvent& event);

  virtual void gripperSliderScrollChanged( wxScrollEvent& event );

  void executeGrasp(DialogOptions options,
                    AdvancedGraspOptions ago);

  void executeRecognizedObjectGrasp(DialogOptions options, 
                                    AdvancedGraspOptions ago,
                                    object_manipulation_msgs::GraspableObject object);

  void executePlace(DialogOptions options,
                    AdvancedGraspOptions ago);

  void modelObject(DialogOptions options);

  void takeMap();

  void moveGripper(int arm_selection_choice, double value);

  void collisionReset(int reset_choice);

  void armMotion(int arm_selection_choice, int arm_action_choice, int arm_planner_choice, bool collision);

  void lookAtTable();
    
  void setStatusLabel(std::string text);

  DialogOptions getDialogOptions();

  void graspableObjectsCallback(const pr2_interactive_object_detection::GraspableObjectListConstPtr &objects);

  void modeledObjectsCallback(const pr2_create_object_model::ObjectInHandConstPtr &object);

  std::string getGraspableObjectName(const object_manipulation_msgs::GraspableObject &obj);

  InteractiveManipulationDisplay *display_;

  ObjectSelectionFrame *object_selection_frame_;

  ros::ServiceClient grasp_planning_srv_;
  ros::ServiceClient place_planning_srv_;
  ros::ServiceClient get_model_description_srv_;
  ros::Subscriber graspable_objects_sub_;
  ros::Subscriber modeled_object_sub_;

  ros::NodeHandle root_nh_;

  ros::NodeHandle priv_nh_;

  boost::thread *executing_thread_;

  boost::mutex mutex_;

  boost::mutex status_label_mutex_;

  boost::mutex object_list_mutex_;

  boost::mutex modeled_object_mutex_;
  
  std::string status_label_text_;

  bool interrupt_request_;

  std::string grasp_planning_service_name_;
  std::string place_planning_service_name_;

  std::string pickup_action_name_;
  std::string place_action_name_;
  
  std::string create_model_action_name_;

  tabletop_collision_map_processing::CollisionMapInterface collision_map_interface_;

  object_manipulator::MechanismInterface mech_interface_;

  GripperController l_gripper_controller_;
  GripperController r_gripper_controller_;

  actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> *pickup_client_;
  actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction> *place_client_;
  actionlib::SimpleActionClient<pr2_create_object_model::ModelObjectInHandAction> *create_model_client_;

  //data from external object detection pipeline
  std::vector<object_manipulation_msgs::GraspableObject> graspable_objects_;

  //buffer needed because of multi-threading
  pr2_interactive_object_detection::GraspableObjectListConstPtr received_objects_;

  bool new_object_list_;

  GraspInfo grasp_info_right_;

  GraspInfo grasp_info_left_;

  AdvancedGraspOptions ago_;
};

}

#endif

---

pr2_interactive_manipulation
Author(s): Matei Ciocarlie
autogenerated on Fri Jan 11 09:11:03 2013