cotesys_ros_grasping: cotesys_ros_grasping: move_arm_relative_cartesian

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 
00043 #include <ros/ros.h>
00044 #include <actionlib/client/simple_action_client.h>
00045 #include <actionlib/server/simple_action_server.h>
00046 #include <tf/tf.h>
00047 #include <tf/transform_listener.h>
00048 
00049 #include <motion_planning_msgs/GetMotionPlan.h>
00050 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00051 #include <cotesys_ros_grasping/MoveArmRelativeCartesianPointAction.h>
00052 
00053 namespace cotesys_ros_grasping
00054 {
00055 
00056 class MoveArmRelativeCartesianPointServer
00057 {
00058 public:
00059   MoveArmRelativeCartesianPointServer();
00060 
00061   bool execute(const cotesys_ros_grasping::MoveArmRelativeCartesianPointGoalConstPtr& goal);
00062 
00063 private:
00064 
00065   std::string left_arm_name_, right_arm_name_;
00066   std::string right_ik_link_, left_ik_link_;
00067 
00068   bool disable_collisions_;
00069 
00070   tf::TransformListener tf_;
00071 
00072   ros::NodeHandle priv_nh_, root_nh_;
00073   ros::ServiceClient left_interp_ik_client_, right_interp_ik_client_; 
00074   boost::shared_ptr<actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> > right_arm_traj_client_, left_arm_traj_client_;
00075   boost::shared_ptr<actionlib::SimpleActionServer<cotesys_ros_grasping::MoveArmRelativeCartesianPointAction> > action_server_;
00076   
00077 };
00078 
00079 MoveArmRelativeCartesianPointServer::MoveArmRelativeCartesianPointServer()
00080   : priv_nh_("~")
00081 {
00082   priv_nh_.param<std::string>("left_arm_name", left_arm_name_, "left_arm");
00083   priv_nh_.param<std::string>("right_arm_name", right_arm_name_, "right_arm");
00084   priv_nh_.param<std::string>("right_ik_link", right_ik_link_, "r_wrist_roll_link");
00085   priv_nh_.param<std::string>("left_ik_link", left_ik_link_, "l_wrist_roll_link");
00086   priv_nh_.param<bool>("disable_collisions", disable_collisions_, false);  
00087 
00088   left_interp_ik_client_ = root_nh_.serviceClient<motion_planning_msgs::GetMotionPlan>("/l_interpolated_ik_motion_plan");
00089   right_interp_ik_client_ = root_nh_.serviceClient<motion_planning_msgs::GetMotionPlan>("/r_interpolated_ik_motion_plan");
00090 
00091   right_arm_traj_client_.reset(new actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>("/r_arm_controller/joint_trajectory_action", true));
00092   left_arm_traj_client_.reset(new actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>("/l_arm_controller/joint_trajectory_action", true));
00093 
00094   //TODO - wait for the servers
00095 
00096   action_server_.reset(new actionlib::SimpleActionServer<cotesys_ros_grasping::MoveArmRelativeCartesianPointAction>(root_nh_, "move_arm_relative_cartesian_point",
00097                                                                                                         boost::bind(&MoveArmRelativeCartesianPointServer::execute, this, _1)));
00098 }
00099 
00100 bool MoveArmRelativeCartesianPointServer::execute(const cotesys_ros_grasping::MoveArmRelativeCartesianPointGoalConstPtr& req)
00101 {
00102   if(req->arm_name != left_arm_name_ && req->arm_name != right_arm_name_) {
00103     ROS_ERROR_STREAM("Can't do anything for arm named " << req->arm_name);
00104   }
00105   
00106   std::string ik_link_name;
00107   ros::ServiceClient* interp_ik_client;
00108   boost::shared_ptr<actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> > joint_traj_client;
00109   if(req->arm_name == left_arm_name_) {
00110     interp_ik_client = &left_interp_ik_client_;
00111     joint_traj_client = left_arm_traj_client_;
00112     ik_link_name = left_ik_link_;
00113   } else {
00114     interp_ik_client = &right_interp_ik_client_;
00115     joint_traj_client = right_arm_traj_client_;
00116     ik_link_name = right_ik_link_;
00117   }
00118 
00119   //now we need to get the current position of the the link in the base_link frame
00120   tf::StampedTransform cur_ik_link_pos;
00121   try {
00122     tf_.lookupTransform("/base_link", "/"+ik_link_name, ros::Time(), cur_ik_link_pos);
00123   } catch(...) {
00124     ROS_WARN("Tf error");
00125   }
00126   geometry_msgs::Point rel_pos;
00127   rel_pos.x = cur_ik_link_pos.getOrigin().x()+req->rel_point.x;
00128   rel_pos.y = cur_ik_link_pos.getOrigin().y()+req->rel_point.y;
00129   rel_pos.z = cur_ik_link_pos.getOrigin().z()+req->rel_point.z;
00130 
00131   motion_planning_msgs::GetMotionPlan::Request plan_req;
00132 
00133   plan_req.motion_plan_request.group_name=req->arm_name;
00134   plan_req.motion_plan_request.num_planning_attempts = 1;
00135   plan_req.motion_plan_request.planner_id = "";
00136   plan_req.motion_plan_request.allowed_planning_time = ros::Duration(2.0);
00137   
00138   //interpolated IK requires start state
00139   plan_req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_id = ik_link_name;
00140   geometry_msgs::Pose pose;
00141   tf::poseTFToMsg(cur_ik_link_pos, pose);
00142   ROS_INFO_STREAM("Ik link name is " << ik_link_name);
00143   plan_req.motion_plan_request.start_state.multi_dof_joint_state.pose = pose;
00144   plan_req.motion_plan_request.start_state.multi_dof_joint_state.frame_id = "base_link";
00145   plan_req.motion_plan_request.start_state.multi_dof_joint_state.stamp = cur_ik_link_pos.stamp_;
00146 
00147   plan_req.motion_plan_request.goal_constraints.position_constraints.resize(1);
00148   plan_req.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00149   plan_req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link";
00150     
00151   plan_req.motion_plan_request.goal_constraints.position_constraints[0].link_name = ik_link_name;
00152   plan_req.motion_plan_request.goal_constraints.position_constraints[0].position.x = rel_pos.x;
00153   plan_req.motion_plan_request.goal_constraints.position_constraints[0].position.y = rel_pos.y;
00154   plan_req.motion_plan_request.goal_constraints.position_constraints[0].position.z = rel_pos.z;
00155     
00156   plan_req.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX;
00157   plan_req.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00158   plan_req.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00159   plan_req.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00160 
00161   plan_req.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00162   plan_req.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00163 
00164   plan_req.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
00165   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00166   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link";
00167   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = ik_link_name;
00168   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = cur_ik_link_pos.getRotation().x();
00169   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = cur_ik_link_pos.getRotation().y();
00170   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = cur_ik_link_pos.getRotation().z();
00171   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = cur_ik_link_pos.getRotation().w();
00172     
00173   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
00174   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
00175   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
00176 
00177   plan_req.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00178 
00179   if(disable_collisions_) {
00180     plan_req.motion_plan_request.ordered_collision_operations.collision_operations.resize(1);
00181     plan_req.motion_plan_request.ordered_collision_operations.collision_operations[0].object1 = 
00182       plan_req.motion_plan_request.ordered_collision_operations.collision_operations[0].COLLISION_SET_ALL;
00183     plan_req.motion_plan_request.ordered_collision_operations.collision_operations[0].object2 = 
00184       plan_req.motion_plan_request.ordered_collision_operations.collision_operations[0].COLLISION_SET_ALL;
00185     plan_req.motion_plan_request.ordered_collision_operations.collision_operations[0].operation = 
00186       plan_req.motion_plan_request.ordered_collision_operations.collision_operations[0].DISABLE;
00187   }
00188 
00189   cotesys_ros_grasping::MoveArmRelativeCartesianPointResult cart_res;
00190 
00191   motion_planning_msgs::GetMotionPlan::Response plan_res;
00192   
00193   if(!interp_ik_client->call(plan_req,plan_res)) {
00194     ROS_WARN_STREAM("Interpolated ik call failed with error code " << plan_res.error_code.val);
00195     cart_res.error_code = plan_res.error_code;
00196     action_server_->setAborted(cart_res);
00197     return true;
00198   }
00199   for(unsigned int i = 0; i < plan_res.trajectory_error_codes.size(); i++) {
00200     if(plan_res.trajectory_error_codes[i].val != plan_res.trajectory_error_codes[i].SUCCESS) {
00201       ROS_WARN_STREAM("Interpolated ik call did not succeed " << plan_res.error_code.val << " for point " << i);
00202       cart_res.error_code = plan_res.trajectory_error_codes[i];
00203       action_server_->setAborted(cart_res);
00204       return true;
00205     }
00206   }
00207 
00208   pr2_controllers_msgs::JointTrajectoryGoal traj_goal;  
00209   traj_goal.trajectory = plan_res.trajectory.joint_trajectory;
00210   
00211   joint_traj_client->sendGoal(traj_goal);
00212 
00213   bool call_ok = joint_traj_client->waitForResult(ros::Duration(30.0));
00214   if(!call_ok) {
00215     ROS_WARN("Call to traj client not ok");
00216   }
00217   
00218   actionlib::SimpleClientGoalState state = joint_traj_client->getState();
00219   if(state != actionlib::SimpleClientGoalState::SUCCEEDED) {
00220     cart_res.error_code.val = cart_res.error_code.TRAJECTORY_CONTROLLER_FAILED;
00221     action_server_->setAborted(cart_res);
00222   } else {
00223     cart_res.error_code.val = cart_res.error_code.SUCCESS;
00224     action_server_->setSucceeded(cart_res);
00225   }
00226   return true;
00227 }
00228 
00229 }
00230 
00231 int main(int argc, char** argv)
00232 {
00233   ros::init(argc, argv, "move_arm_relative_cartesian_point");
00234   
00235   ros::AsyncSpinner spinner(1); // Use 1 thread
00236   spinner.start();
00237 
00238   cotesys_ros_grasping::MoveArmRelativeCartesianPointServer move_arm_pos;
00239 
00240   ROS_INFO("Move_arm_to_position action started");
00241   ros::waitForShutdown();
00242     
00243   return 0;
00244 }
move_arm_relative_cartesian_point.cpp
