attach_bounding_box_server.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author E. Gil Jones
00036  *********************************************************************/
00037 
00038 #include <stdio.h>
00039 #include <stdlib.h>
00040 #include <time.h>
00041 #include <boost/thread.hpp>
00042 #include <sstream>
00043 
00044 #include <ros/ros.h>
00045 #include <tf/tf.h>
00046 #include <tf/transform_listener.h>
00047 #include <actionlib/client/simple_action_client.h>
00048 #include <actionlib/server/simple_action_server.h>
00049 #include <mapping_msgs/AttachedCollisionObject.h>
00050 
00051 #include <cotesys_ros_grasping/AttachBoundingBoxAction.h>
00052 
00053 namespace cotesys_ros_grasping
00054 {
00055 
00056 class AttachBoundingBoxServer
00057 {
00058 public:
00059   
00060   AttachBoundingBoxServer();
00061 
00062   bool execute(const cotesys_ros_grasping::AttachBoundingBoxGoalConstPtr& goal);
00063 
00064 private:
00065   
00066   boost::shared_ptr<actionlib::SimpleActionServer<cotesys_ros_grasping::AttachBoundingBoxAction> > action_server_;
00067   ros::NodeHandle priv_nh_, root_nh_;
00068   ros::Publisher attached_object_publisher_;
00069 
00070   std::string default_object_name_;
00071   std::string left_arm_name_, right_arm_name_;
00072   std::string right_attach_link_, left_attach_link_;
00073 
00074   std::vector<std::string> left_end_effector_links_;
00075   std::vector<std::string> right_end_effector_links_;
00076 
00077   std::string left_end_effector_planning_group_, right_end_effector_planning_group_;
00078 
00079   double link_to_gripper_x_diff_, link_to_gripper_y_diff_, link_to_gripper_z_diff_;
00080 
00081   tf::TransformListener tf_;
00082 };
00083 
00084 AttachBoundingBoxServer::AttachBoundingBoxServer()
00085   : priv_nh_("~")
00086 {
00087   priv_nh_.param<std::string>("left_arm_name", left_arm_name_, "left_arm");
00088   priv_nh_.param<std::string>("right_arm_name", right_arm_name_, "right_arm");
00089   priv_nh_.param<std::string>("default_object_name", default_object_name_, "_object");
00090   priv_nh_.param<std::string>("right_attach_link", right_attach_link_, "r_gripper_palm_link");
00091   priv_nh_.param<std::string>("left_attach_link", left_attach_link_, "l_gripper_palm_link");
00092   priv_nh_.param<std::string>("left_end_effector_planning_group", left_end_effector_planning_group_,"l_end_effector");
00093   priv_nh_.param<std::string>("right_end_effector_planning_group", right_end_effector_planning_group_,"r_end_effector");
00094   priv_nh_.param<double>("link_to_gripper_x_diff", link_to_gripper_x_diff_, .06);
00095   priv_nh_.param<double>("link_to_gripper_y_diff", link_to_gripper_y_diff_, .00);
00096   priv_nh_.param<double>("link_to_gripper_z_diff", link_to_gripper_z_diff_, .00);
00097 
00098   //getting end effector link members
00099   XmlRpc::XmlRpcValue group_list;
00100   root_nh_.getParam("/robot_description_planning/groups", group_list);
00101   if(group_list.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00102     ROS_WARN("Problem getting group list");
00103   }
00104   bool found_left = false;
00105   bool found_right = false;
00106   for(int i = 0; i < group_list.size(); i++) {
00107     std::vector<std::string>* group_link_vector = NULL;
00108   
00109     if(group_list[i]["name"] == left_end_effector_planning_group_) {
00110       found_left = true;
00111       group_link_vector = &left_end_effector_links_;
00112     } else if(group_list[i]["name"] == right_end_effector_planning_group_) {
00113       found_right = true;
00114       group_link_vector = &right_end_effector_links_;
00115     }
00116     if(group_link_vector != NULL) {
00117       if(!group_list[i].hasMember("links")) {
00118         ROS_WARN("No links in end effector group");
00119         continue;
00120       }
00121       std::string link_list = std::string(group_list[i]["links"]);
00122       std::stringstream link_name_stream(link_list);
00123       while(link_name_stream.good() && !link_name_stream.eof()){
00124         std::string lname; 
00125         link_name_stream >> lname;
00126         if(lname.size() == 0) continue;
00127         group_link_vector->push_back(lname);
00128       }
00129     }
00130   }
00131   if(!found_right) {
00132     ROS_INFO_STREAM("Couldn't find planning group for " << right_end_effector_planning_group_);
00133   }
00134 
00135   if(!found_left) {
00136     ROS_INFO_STREAM("Couldn't find planning group for " << left_end_effector_planning_group_);
00137   }
00138   
00139   attached_object_publisher_ = root_nh_.advertise<mapping_msgs::AttachedCollisionObject>("attached_collision_object", true);
00140   
00141   action_server_.reset(new actionlib::SimpleActionServer<cotesys_ros_grasping::AttachBoundingBoxAction>(root_nh_, "attach_bounding_box",
00142                                                                                                         boost::bind(&AttachBoundingBoxServer::execute, this, _1)));
00143 };
00144 
00145 bool AttachBoundingBoxServer::execute(const cotesys_ros_grasping::AttachBoundingBoxGoalConstPtr& req)
00146 {
00147   if(req->arm_name != left_arm_name_ && req->arm_name != right_arm_name_) {
00148     ROS_ERROR_STREAM("Can't attach to arm named " << req->arm_name);
00149     action_server_->setAborted();
00150     return true;
00151   }
00152 
00153   std::string att_link_name;
00154   std::vector<std::string>* touch_link_vector;
00155   if(req->arm_name == left_arm_name_) {
00156     att_link_name = left_attach_link_;
00157     touch_link_vector = &left_end_effector_links_;
00158   } else {
00159     att_link_name = right_attach_link_;
00160     touch_link_vector = &right_end_effector_links_;
00161   }
00162 
00163   mapping_msgs::AttachedCollisionObject att_object;
00164 
00165   att_object.link_name = att_link_name;
00166   att_object.object = req->object;
00167   if(req->remove) {
00168     att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::REMOVE;
00169   } else {
00170     att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::ATTACH_AND_REMOVE_AS_OBJECT;
00171 
00172     att_object.touch_links = *touch_link_vector;
00173   }
00174 
00175   attached_object_publisher_.publish(att_object);
00176 
00177   action_server_->setSucceeded();
00178   return true;
00179 }
00180 
00181 }
00182 
00183 int main(int argc, char** argv)
00184 {
00185   ros::init(argc, argv, "attach_bounding_box_server");
00186   
00187   ros::AsyncSpinner spinner(1); // Use 1 thread
00188   spinner.start();
00189 
00190   cotesys_ros_grasping::AttachBoundingBoxServer attach_bounding_box;
00191 
00192   ROS_INFO("Attach bounding box server started");
00193   ros::waitForShutdown();
00194     
00195   return 0;
00196 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


cotesys_ros_grasping
Author(s): Gil Jones
autogenerated on Thu May 23 2013 10:22:37