simple_move_arm_server.cpp
Go to the documentation of this file.
00001 /* 
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2011, Vanadium Labs LLC.
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions
00009  * are met:
00010  *
00011  *  * Redistributions of source code must retain the above copyright
00012  *    notice, this list of conditions and the following disclaimer.
00013  *  * Redistributions in binary form must reproduce the above
00014  *    copyright notice, this list of conditions and the following
00015  *    disclaimer in the documentation and/or other materials provided
00016  *    with the distribution.
00017  *  * Neither the name of Vanadium Labs LLC nor the names of its
00018  *    contributors may be used to endorse or promote prducts derived
00019  *    from this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 /*
00036  * author: Michael Ferguson
00037  */
00038 
00039 #include <ros/ros.h>
00040 #include <tf/tf.h>
00041 #include <tf/transform_listener.h>
00042 #include <actionlib/server/simple_action_server.h>
00043 #include <actionlib/client/simple_action_client.h>
00044 #include <arm_navigation_msgs/MoveArmAction.h>
00045 #include <arm_navigation_msgs/utils.h>
00046 #include <simple_arm_server/MoveArmAction.h>
00047 #include <std_msgs/Float64.h>
00048 #include <math.h>
00049 
00050 class SimpleArmNavigationServer
00051 {
00052   private:
00053     ros::NodeHandle nh;
00054     tf::TransformListener listener;
00055 
00056     // ROS interface
00057     actionlib::SimpleActionServer<simple_arm_server::MoveArmAction> server;
00058     actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> client;
00059     ros::Publisher gripper;
00060 
00061     simple_arm_server::MoveArmFeedback     feedback_;
00062     simple_arm_server::MoveArmResult       result_;
00063     simple_arm_server::MoveArmGoalConstPtr goal_;
00064 
00065     // parameters
00066     int arm_dof_;
00067     std::string root_name_;
00068     std::string tip_name_;
00069     std::string pan_link_;
00070     double timeout_;
00071     double x_offset_;
00072     int max_tries_;
00073     std::vector<double> step_list_;
00074 
00075   public:
00076     
00077     SimpleArmNavigationServer() : nh("~"), listener(nh), server(ros::NodeHandle(), "simple_move_arm", false), client("move_arm", true)
00078     {
00079         ROS_INFO("Starting simple_move_arm_server.");
00080         // configuration
00081         nh.param<int>("arm_dof", arm_dof_, 5);
00082         nh.param<std::string>("root_name", root_name_, "arm_link");
00083         nh.param<std::string>("tip_name", tip_name_, "gripper_link");
00084         nh.param<double>("timeout", timeout_, 5.0);
00085         nh.param<int>("iterations", max_tries_, 30);
00086         if(nh.hasParam("step_list")){
00087             XmlRpc::XmlRpcValue step_params;
00088             nh.getParam("step_list", step_params);
00089             for(int i=0; i<step_params.size(); ++i)
00090             {
00091                 step_list_.push_back( static_cast<double>(step_params[i]) );
00092             }
00093         }else{
00094             step_list_.push_back(-0.05);
00095             step_list_.push_back( 0.05);
00096         }
00097 
00098         // lookup X offset of pan_link from root_name
00099         nh.param<std::string>("pan_link", pan_link_, "arm_link");
00100         if( !listener.waitForTransform(root_name_, pan_link_, ros::Time(0), ros::Duration(10.0)) ){
00101             ROS_ERROR("Cannot lookup transfrom from %s to %s", pan_link_.c_str(), root_name_.c_str());
00102         }
00103         tf::StampedTransform transform;
00104         try{
00105             listener.lookupTransform(root_name_, pan_link_, ros::Time(0), transform);
00106         }catch(tf::TransformException ex){
00107             ROS_ERROR("%s",ex.what());
00108         }
00109         x_offset_ = transform.getOrigin().x();
00110         ROS_INFO("X offset %f", x_offset_);
00111 
00112         // output for arm movement
00113         client.waitForServer();
00114         gripper = nh.advertise<std_msgs::Float64>("/gripper_controller/command", 1, false);
00115         ROS_INFO("Connected to move_arm server");
00116 
00117         // input for moving arm
00118         server.registerGoalCallback(boost::bind(&SimpleArmNavigationServer::goalCB, this));
00119         server.registerPreemptCallback(boost::bind(&SimpleArmNavigationServer::preemptCB, this));
00120         server.start();
00121         
00122         ROS_INFO("simple_move_arm_server node started");
00123     }
00124 
00125     void goalCB()
00126     {
00127         // accept the new goal
00128         goal_ = server.acceptNewGoal();
00129         
00130         for(size_t i = 0; i < goal_->motions.size(); i++){
00131             simple_arm_server::ArmAction action = goal_->motions[i];
00132 
00133             if( action.type == simple_arm_server::ArmAction::MOVE_ARM )
00134             {
00135                 // transform goal
00136                 geometry_msgs::PoseStamped in, pose;
00137                 in.header = goal_->header;
00138                 in.pose = action.goal;
00139                 try{
00140                     listener.transformPose(root_name_, in, pose);
00141                 }catch(tf::TransformException ex){
00142                     ROS_ERROR("%s",ex.what());
00143                 }
00144                 // our hacks for low DOF
00145                 double roll, pitch, yaw;
00146                 btQuaternion q(pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
00147                 btMatrix3x3(q).getRPY(roll, pitch, yaw);
00148                 if( arm_dof_ < 6 ){
00149                     // 5DOF, so yaw angle = atan2(Y,X-shoulder offset)
00150                     yaw = atan2(pose.pose.position.y, pose.pose.position.x-x_offset_);
00151                     if( arm_dof_ < 5 ){
00152                         // 4 DOF, so yaw as above AND no roll
00153                         roll = 0.0;
00154                     }
00155                 }
00156 
00157                 // wiggle if needed
00158                 int tries;
00159                 bool goal_reached = false;
00160                 for( tries = 0; tries <= max_tries_; tries++ ){
00161                     for(size_t i=0; i<step_list_.size(); i++){
00162                         // construct the representation of a pose goal (define the position of the end effector)
00163                         arm_navigation_msgs::MoveArmGoal goal;
00164                         
00165                         goal.motion_plan_request.group_name = "arm";
00166                         goal.motion_plan_request.num_planning_attempts = 1;
00167                         goal.motion_plan_request.planner_id = std::string("");
00168                         goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
00169                         goal.motion_plan_request.allowed_planning_time = ros::Duration(timeout_);
00170 
00171                         arm_navigation_msgs::SimplePoseConstraint desired_pose;
00172                         desired_pose.absolute_position_tolerance.x = 0.005;
00173                         desired_pose.absolute_position_tolerance.y = 0.005;
00174                         desired_pose.absolute_position_tolerance.z = 0.005;
00175                         desired_pose.absolute_roll_tolerance = 0.25;
00176                         desired_pose.absolute_pitch_tolerance = 0.05;
00177                         desired_pose.absolute_yaw_tolerance = 0.5;
00178 
00179                         desired_pose.header.frame_id = pose.header.frame_id;
00180                         desired_pose.link_name = tip_name_;
00181 
00182                         desired_pose.pose.position.x = pose.pose.position.x;
00183                         desired_pose.pose.position.y = pose.pose.position.y;
00184                         desired_pose.pose.position.z = pose.pose.position.z;
00185 
00186                         double attempt = pitch + (tries * step_list_[i]);
00187                         q.setRPY(roll, attempt, yaw);
00188 
00189                         desired_pose.pose.orientation.x = (double) q.getX();
00190                         desired_pose.pose.orientation.y = (double) q.getY();
00191                         desired_pose.pose.orientation.z = (double) q.getZ();
00192                         desired_pose.pose.orientation.w = (double) q.getW();
00193                         ROS_INFO("%d: (%f, %f, %f)", tries, roll, attempt, yaw);
00194 
00195                         arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose, goal);
00196 
00197                         // send request
00198                         client.sendGoal(goal);
00199                         bool finished_within_time = client.waitForResult(ros::Duration(300.0));
00200                         if( !finished_within_time )
00201                         {
00202                             client.cancelGoal();
00203                             ROS_INFO("Continue: goal timed out");
00204                         }
00205                         else
00206                         {
00207                             actionlib::SimpleClientGoalState state = client.getState();
00208                             arm_navigation_msgs::MoveArmResultConstPtr result = client.getResult();
00209                             //if(state == actionlib::SimpleClientGoalState::SUCCEEDED){
00210                             if(result->error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS ){
00211                                 ROS_INFO("Continue: goal succeeded.");
00212                                 goal_reached = true;
00213                             }else if( result->error_code.val == int(arm_navigation_msgs::ArmNavigationErrorCodes::START_STATE_IN_COLLISION) ){
00214                                 ROS_INFO("Abort: start state in collision.");
00215                                 result_.success = false;
00216                                 server.setAborted(result_);
00217                                 return;
00218                             }else if( (result->error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::GOAL_CONSTRAINTS_VIOLATED) ||
00219                                       (result->error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::PATH_CONSTRAINTS_VIOLATED) ) {
00220                                 ROS_INFO("Continue: goal constraints violated.");
00221                                 goal_reached = true;
00222                             }else{
00223                                 ROS_INFO("Continue: path failed, trying again");
00224                             }
00225                         }
00226                         if(tries==0) break;
00227                             if(goal_reached) break;
00228                     } // end of steps
00229                         if(goal_reached) break;
00230                 } // end of tries                   
00231                 if( tries > max_tries_ ){
00232                     result_.success = false;
00233                     server.setAborted(result_);
00234                     return;                    
00235                 }
00236             }
00237             else if( action.type == simple_arm_server::ArmAction::MOVE_GRIPPER )
00238             {
00239                 ROS_INFO("Move gripper to %f.", action.command);
00240                 std_msgs::Float64 msg;
00241                 msg.data = action.command;
00242                 gripper.publish( msg );
00243                 ros::Duration(action.move_time).sleep();
00244             }
00245         } // for each action
00246 
00247         result_.success = true;
00248         server.setSucceeded(result_);
00249     }
00250 
00251     void preemptCB()
00252     {
00253         ROS_INFO("simple_move_arm preempted");
00254         server.setPreempted();
00255     }
00256 
00257 };
00258 
00259 int main(int argc, char **argv)
00260 {
00261     ros::init (argc, argv, "simple_move_arm_server");
00262     
00263     SimpleArmNavigationServer server;
00264     ros::spin();
00265     
00266     return 0;
00267 }
00268 


simple_arm_server
Author(s): Michael Ferguson
autogenerated on Fri Jan 3 2014 11:58:44