block_manipulation_actions.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 
00031 #include <ros/ros.h>
00032 
00033 #include <actionlib/client/simple_action_client.h>
00034 #include <turtlebot_block_manipulation/BlockDetectionAction.h>
00035 #include <turtlebot_block_manipulation/PickAndPlaceAction.h>
00036 
00037 #include <interactive_markers/interactive_marker_server.h>
00038 
00039 
00040 using namespace visualization_msgs;
00041 
00042 const std::string arm_link = "/arm_base_link";
00043 const double gripper_open = 0.042;
00044 const double gripper_closed = 0.024;
00045 
00046 const double z_up = 0.08;
00047 const double z_down = -0.05;
00048 
00049 const double block_size = 0.0127;
00050 
00051 
00052 class BlockManipulationAction
00053 {
00054 private:
00055   interactive_markers::InteractiveMarkerServer server_;
00056   
00057   ros::NodeHandle nh_;
00058   
00059   // Actions
00060   actionlib::SimpleActionClient<turtlebot_block_manipulation::BlockDetectionAction> block_detection_action_;
00061   actionlib::SimpleActionClient<turtlebot_block_manipulation::PickAndPlaceAction> pick_and_place_action_;
00062   
00063   turtlebot_block_manipulation::BlockDetectionGoal block_detection_goal_;
00064   turtlebot_block_manipulation::PickAndPlaceGoal pick_and_place_goal_;
00065   
00066   geometry_msgs::Pose old_pose_;
00067 
00068 public:
00069 
00070   BlockManipulationAction() : 
00071     server_("block_controls"), 
00072     block_detection_action_("block_detection", true),
00073     pick_and_place_action_("pick_and_place", true)
00074   {
00075     server_.applyChanges();
00076     
00077     // Initialize goals
00078     block_detection_goal_.frame = arm_link;
00079     block_detection_goal_.table_height = z_down;
00080     block_detection_goal_.block_size = block_size;
00081     
00082     pick_and_place_goal_.frame = arm_link;
00083     pick_and_place_goal_.z_up = z_up;
00084     pick_and_place_goal_.gripper_open = gripper_open;
00085     pick_and_place_goal_.gripper_closed = gripper_closed;
00086     
00087     ROS_INFO("Finished initializing, waiting for servers...");
00088     
00089     block_detection_action_.waitForServer();
00090     pick_and_place_action_.waitForServer();
00091     
00092     ROS_INFO("Found servers.");
00093     
00094     detectBlocks();
00095   }
00096   
00097   void detectBlocks()
00098   {
00099     server_.clear();
00100     server_.applyChanges();
00101     
00102     block_detection_action_.sendGoal(block_detection_goal_,  boost::bind( &BlockManipulationAction::addBlocks, this, _1, _2));
00103   }
00104   
00105   void addBlocks(const actionlib::SimpleClientGoalState& state, const turtlebot_block_manipulation::BlockDetectionResultConstPtr& result)
00106   {
00107     ROS_INFO("Got block detection callback. Adding blocks.");
00108     geometry_msgs::Pose block;
00109     
00110     for (unsigned int i=0; i < result->blocks.poses.size(); i++)
00111     {
00112       block = result->blocks.poses[i];
00113       addBlock(block.position.x, block.position.y, block.position.z, i);
00114       ROS_INFO("Added %d blocks", i);
00115     }
00116     
00117     server_.applyChanges(); 
00118   }
00119   
00120 
00121   // Move the real block!
00122   void feedbackCb( const InteractiveMarkerFeedbackConstPtr &feedback )
00123   {
00124     switch ( feedback->event_type )
00125     {
00126       case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
00127         ROS_INFO_STREAM("Staging " << feedback->marker_name);     
00128           old_pose_ = feedback->pose;
00129         break;
00130    
00131       case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
00132         ROS_INFO_STREAM("Now moving " << feedback->marker_name); 
00133         moveBlock(old_pose_, feedback->pose);
00134         detectBlocks();
00135         break;
00136     }
00137     
00138     server_.applyChanges(); 
00139   }
00140   
00141   bool moveBlock(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
00142   {
00143     pick_and_place_goal_.pickup_pose = start_pose;
00144     pick_and_place_goal_.place_pose = end_pose;
00145     pick_and_place_action_.sendGoalAndWait(pick_and_place_goal_, ros::Duration(30.0), ros::Duration(30.0));
00146     //pick_and_place_action_.sendGoal(pick_and_place_goal_);
00147     //pick_and_place_action_.waitForResult(ros::Duration(30.0));
00148     
00149     if ( block_detection_action_.getState()  == actionlib::SimpleClientGoalState::SUCCEEDED)
00150     {
00151       return true;
00152     }
00153     else
00154     {
00155       return false;
00156     }
00157   }
00158 
00159   // Make a box
00160   Marker makeBox( InteractiveMarker &msg, float r, float g, float b )
00161   {
00162     Marker m;
00163 
00164     m.type = Marker::CUBE;
00165     m.scale.x = msg.scale;
00166     m.scale.y = msg.scale;
00167     m.scale.z = msg.scale;
00168     m.color.r = r;
00169     m.color.g = g;
00170     m.color.b = b;
00171     m.color.a = 1.0;
00172 
00173     return m;
00174   }
00175    
00176   // Add a new block
00177   void addBlock( float x, float y, float z, int n)
00178   {
00179     InteractiveMarker marker;
00180     marker.header.frame_id = arm_link;
00181     marker.pose.position.x = x;
00182     marker.pose.position.y = y;
00183     marker.pose.position.z = z;
00184     marker.scale = 0.03;
00185     
00186     std::stringstream conv;
00187     conv << n;
00188     conv.str();
00189      
00190     marker.name = "block" + conv.str();
00191 
00192     InteractiveMarkerControl control;
00193     control.orientation.w = 1;
00194     control.orientation.x = 0;
00195     control.orientation.y = 1;
00196     control.orientation.z = 0;
00197     control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00198     marker.controls.push_back( control );
00199     
00200     control.markers.push_back( makeBox(marker, .5, .5, .5) );
00201     control.always_visible = true;
00202     marker.controls.push_back( control );
00203     
00204 
00205     server_.insert( marker );
00206     server_.setCallback( marker.name, boost::bind( &BlockManipulationAction::feedbackCb, this, _1 ));
00207   }
00208 
00209 };
00210 
00211 int main(int argc, char** argv)
00212 {
00213   // initialize node
00214   ros::init(argc, argv, "block_manipulation");
00215 
00216   BlockManipulationAction manip;
00217 
00218   // everything is done in cloud callback, just spin
00219   ros::spin();
00220 }
00221 


turtlebot_block_manipulation
Author(s): Michael Ferguson, Helen Oleynikova
autogenerated on Mon Oct 6 2014 08:07:45