grasp_caller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* Simply calls the grasp action of the interactive manipulation backend, again
00031    and again until the end of time. */
00032 
00033 #include <ros/ros.h>
00034 
00035 #include <actionlib/client/simple_action_client.h>
00036 
00037 #include <pr2_interactive_manipulation/advanced_options_dialog.h>
00038 #include <pr2_object_manipulation_msgs/IMGUIOptions.h>
00039 #include <pr2_object_manipulation_msgs/IMGUIAdvancedOptions.h>
00040 #include <pr2_object_manipulation_msgs/IMGUIAction.h>
00041 
00042 using namespace pr2_interactive_manipulation;
00043 
00044 class GraspCaller
00045 {
00046 private:
00047   ros::NodeHandle nh_;
00048   ros::Timer slow_sync_timer_;
00049   actionlib::SimpleActionClient<pr2_object_manipulation_msgs::IMGUIAction> *imgui_action_client_;
00050   int task_number_;
00051 
00052 public:
00053   GraspCaller() : nh_("") 
00054   {
00055     imgui_action_client_ = new actionlib::SimpleActionClient<pr2_object_manipulation_msgs::IMGUIAction>("imgui_action", true);
00056     slow_sync_timer_ =  nh_.createTimer(ros::Duration(0.5), boost::bind( &GraspCaller::slowSync, this ) );
00057     nh_.param<int>("interactive_grasping/task_number", task_number_, 0);
00058   }
00059 
00060   void slowSync()
00061   {
00062 
00063     pr2_object_manipulation_msgs::IMGUIAdvancedOptions adv_options;
00064     adv_options = AdvancedOptionsDialog::getDefaultsMsg();
00065     if (task_number_ == 3) adv_options.lift_direction_choice = 1;
00066 
00067     pr2_object_manipulation_msgs::IMGUIGoal imgui_goal;
00068     imgui_goal.options.collision_checked = true;
00069     imgui_goal.options.grasp_selection = 0;
00070     imgui_goal.options.arm_selection = 0;
00071     imgui_goal.options.adv_options = adv_options;
00072     imgui_goal.command.command = imgui_goal.command.PICKUP;
00073 
00074     actionlib::SimpleClientGoalState state = imgui_action_client_->getState();
00075     if (state != actionlib::SimpleClientGoalState::PENDING && 
00076         state != actionlib::SimpleClientGoalState::ACTIVE)
00077     {
00078       imgui_action_client_->sendGoal(imgui_goal);
00079     }
00080   }
00081 
00082 };
00083 
00084 int main (int argc, char **argv)
00085 {
00086   ros::init(argc, argv, "grasp_caller");
00087   GraspCaller caller;
00088   ros::spin();
00089 }


pr2_interactive_manipulation
Author(s): Matei Ciocarlie, Kaijen Hsiao, Adam Leeper
autogenerated on Fri Jan 3 2014 12:08:58