interactive_manipulation_action_server.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  * Author: Michael Ferguson, Helen Oleynikova
00030  */
00031 
00032 #include <ros/ros.h>
00033 
00034 #include <actionlib/server/simple_action_server.h>
00035 #include <interactive_markers/interactive_marker_server.h>
00036 
00037 #include <turtlebot_arm_block_manipulation/InteractiveBlockManipulationAction.h>
00038 #include <geometry_msgs/PoseArray.h>
00039 
00040 using namespace visualization_msgs;
00041 
00042 namespace turtlebot_arm_block_manipulation
00043 {
00044 
00045 class InteractiveManipulationServer
00046 {
00047 private:
00048   ros::NodeHandle nh_;
00049 
00050   interactive_markers::InteractiveMarkerServer server_;
00051   
00052   actionlib::SimpleActionServer<turtlebot_arm_block_manipulation::InteractiveBlockManipulationAction> as_;
00053   std::string action_name_;
00054   
00055   turtlebot_arm_block_manipulation::InteractiveBlockManipulationFeedback     feedback_;
00056   turtlebot_arm_block_manipulation::InteractiveBlockManipulationResult       result_;
00057   turtlebot_arm_block_manipulation::InteractiveBlockManipulationGoalConstPtr goal_;
00058   
00059   ros::Subscriber block_sub_;
00060   ros::Publisher  pick_and_place_pub_;
00061 
00062   geometry_msgs::Pose old_pose_;
00063   
00064   geometry_msgs::PoseArrayConstPtr msg_;
00065   bool initialized_;
00066   
00067   // Parameters from goal
00068   std::string arm_link;
00069   double      block_size;
00070   
00071   // Parameters from server
00072   double bump_size;
00073 
00074 public:
00075 
00076   InteractiveManipulationServer(const std::string name) : 
00077      nh_("~"), server_("block_controls"), as_(name, false), action_name_(name), initialized_(false), block_size(0)
00078   {
00079     // Load parameters from the server.
00080     nh_.param<double>("bump_size", bump_size, 0.005);
00081     
00082     // Register the goal and feeback callbacks.
00083     as_.registerGoalCallback(boost::bind(&InteractiveManipulationServer::goalCB, this));
00084     as_.registerPreemptCallback(boost::bind(&InteractiveManipulationServer::preemptCB, this));
00085     
00086     as_.start();
00087   
00088     block_sub_ = nh_.subscribe("/turtlebot_blocks", 1, &InteractiveManipulationServer::addBlocks, this);
00089     pick_and_place_pub_ = nh_.advertise< geometry_msgs::PoseArray >("/pick_and_place", 1, true);
00090   }
00091   
00092   void goalCB()
00093   {
00094 
00095     // accept the new goal
00096     goal_ = as_.acceptNewGoal();
00097     
00098     ROS_INFO("[interactive manipulation] Received goal! %f, %s", goal_->block_size, goal_->frame.c_str());
00099     
00100     block_size = goal_->block_size;
00101     arm_link = goal_->frame;
00102     
00103     if (initialized_)
00104       addBlocks(msg_);
00105   }
00106 
00107   void preemptCB()
00108   {
00109     ROS_INFO("%s: Preempted", action_name_.c_str());
00110     // set the action state to preempted
00111     as_.setPreempted();
00112   }
00113   
00114   void addBlocks(const geometry_msgs::PoseArrayConstPtr& msg)
00115   {
00116     server_.clear();
00117     server_.applyChanges();
00118   
00119     ROS_INFO("Got block detection callback. Adding blocks.");
00120     geometry_msgs::Pose block;
00121     bool active = as_.isActive();
00122     
00123     for (unsigned int i=0; i < msg->poses.size(); i++)
00124     {
00125       block = msg->poses[i];
00126       addBlock(block, i, active, msg->header.frame_id);
00127       ROS_INFO("Added %d blocks", i);
00128     }
00129     
00130     server_.applyChanges();
00131     
00132     msg_ = msg;
00133     initialized_ = true;
00134   }
00135   
00136 
00137   // Move the real block!
00138   void feedbackCb( const InteractiveMarkerFeedbackConstPtr &feedback )
00139   {
00140     if (!as_.isActive())
00141     {
00142       ROS_INFO("Got feedback but not active!");
00143       return;
00144     }
00145     switch ( feedback->event_type )
00146     {
00147       case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
00148         ROS_INFO_STREAM("Staging " << feedback->marker_name);     
00149         old_pose_ = feedback->pose;
00150         break;
00151    
00152       case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
00153         ROS_INFO_STREAM("Now moving " << feedback->marker_name); 
00154         moveBlock(old_pose_, feedback->pose);
00155         break;
00156     }
00157     
00158     server_.applyChanges(); 
00159   }
00160   
00161   void moveBlock(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
00162   {
00163     // Return pickup and place poses as the result of the action
00164     geometry_msgs::Pose start_pose_bumped, end_pose_bumped;
00165     start_pose_bumped = start_pose;
00166     //  start_pose_bumped.position.y -= bump_size;  TODO/WARN: this was in the original version, but destroys piss off indigo/moveit operation!
00167     start_pose_bumped.position.z -= block_size/2.0 - bump_size;
00168 
00169     result_.pickup_pose = start_pose_bumped;
00170     
00171     end_pose_bumped = end_pose;
00172     end_pose_bumped.position.z -= block_size/2.0 - bump_size;
00173     result_.place_pose = end_pose_bumped;
00174     
00175     // Publish pickup and place poses for visualizing on RViz
00176     geometry_msgs::PoseArray msg;
00177     msg.header.frame_id = arm_link;
00178     msg.header.stamp = ros::Time::now();
00179     msg.poses.push_back(start_pose_bumped);
00180     msg.poses.push_back(end_pose_bumped);
00181     
00182     pick_and_place_pub_.publish(msg);
00183     
00184     as_.setSucceeded(result_);
00185     
00186     server_.clear();
00187     server_.applyChanges();
00188   }
00189 
00190   // Make a box
00191   Marker makeBox( InteractiveMarker &msg, float r, float g, float b )
00192   {
00193     Marker m;
00194 
00195     m.type = Marker::CUBE;
00196     m.scale.x = msg.scale;
00197     m.scale.y = msg.scale;
00198     m.scale.z = msg.scale;
00199     m.color.r = r;
00200     m.color.g = g;
00201     m.color.b = b;
00202     m.color.a = 1.0;
00203 
00204     return m;
00205   }
00206    
00207   // Add a new block
00208   void addBlock( const geometry_msgs::Pose pose, int n, bool active, std::string link)
00209   {
00210     InteractiveMarker marker;
00211     marker.header.frame_id = link;
00212     marker.pose = pose;
00213     marker.scale = block_size;
00214     
00215     std::stringstream conv;
00216     conv << n;
00217     conv.str();
00218      
00219     marker.name = "block" + conv.str();
00220 
00221     InteractiveMarkerControl control;
00222     control.orientation.w = 1;
00223     control.orientation.x = 0;
00224     control.orientation.y = 1;
00225     control.orientation.z = 0;
00226     control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00227     
00228     if (active)
00229       marker.controls.push_back( control );
00230     
00231     control.markers.push_back( makeBox(marker, .5, .5, .5) );
00232     control.always_visible = true;
00233     marker.controls.push_back( control );
00234     
00235 
00236     server_.insert( marker );
00237     server_.setCallback( marker.name, boost::bind( &InteractiveManipulationServer::feedbackCb, this, _1 ));
00238   }
00239 
00240 };
00241 
00242 };
00243 
00244 int main(int argc, char** argv)
00245 {
00246   // initialize node
00247   ros::init(argc, argv, "interactive_manipulation_action_server");
00248 
00249   turtlebot_arm_block_manipulation::InteractiveManipulationServer manip("interactive_manipulation");
00250 
00251   ros::spin();
00252 }
00253 


turtlebot_arm_block_manipulation
Author(s): Michael Ferguson, Helen Oleynikova
autogenerated on Thu Jun 6 2019 20:54:14