move_arm_simple_action.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 Sachin Chitta, Ioan Sucan
00036  *********************************************************************/
00037 
00038 #include <ros/ros.h>
00039 
00040 #include <tf/tf.h>
00041 #include <tf/transform_listener.h>
00042 #include <urdf/model.h>
00043 #include <angles/angles.h>
00044 
00045 #include <actionlib/client/action_client.h>
00046 #include <control_msgs/FollowJointTrajectoryAction.h>
00047 
00048 #include <actionlib/server/simple_action_server.h>
00049 #include <arm_navigation_msgs/MoveArmStatistics.h>
00050 #include <arm_navigation_msgs/MoveArmAction.h>
00051 
00052 #include <trajectory_msgs/JointTrajectory.h>
00053 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00054 #include <kinematics_msgs/GetPositionFK.h>
00055 
00056 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00057 #include <arm_navigation_msgs/Shape.h>
00058 #include <arm_navigation_msgs/DisplayTrajectory.h>
00059 #include <arm_navigation_msgs/GetMotionPlan.h>
00060 #include <arm_navigation_msgs/convert_messages.h>
00061 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00062 
00063 #include <visualization_msgs/Marker.h>
00064 
00065 #include <planning_environment/util/construct_object.h>
00066 #include <arm_navigation_msgs/utils.h>
00067 #include <geometric_shapes/bodies.h>
00068 
00069 #include <visualization_msgs/MarkerArray.h>
00070 
00071 #include <planning_environment/models/collision_models.h>
00072 #include <planning_environment/models/model_utils.h>
00073 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00074 
00075 #include <arm_navigation_msgs/GetRobotState.h>
00076 
00077 #include <actionlib/client/simple_action_client.h>
00078 #include <actionlib/client/simple_client_goal_state.h>
00079 
00080 #include <std_msgs/Bool.h>
00081 
00082 #include <valarray>
00083 #include <algorithm>
00084 #include <cstdlib>
00085 
00086 typedef actionlib::ActionClient<control_msgs::FollowJointTrajectoryAction> JointExecutorActionClient;
00087 
00088 namespace move_arm
00089 {
00090 
00091 enum MoveArmState {
00092   PLANNING,
00093   START_CONTROL,
00094   VISUALIZE_PLAN,
00095   MONITOR
00096 };
00097 
00098 enum ControllerStatus {
00099   QUEUED,
00100   ACTIVE,
00101   SUCCESS,
00102   FAILED
00103 };
00104 
00105 enum EnvironmentServerChecks{
00106   COLLISION_TEST        = 1,
00107   PATH_CONSTRAINTS_TEST = 2,
00108   GOAL_CONSTRAINTS_TEST = 4,
00109   JOINT_LIMITS_TEST     = 8,
00110   CHECK_FULL_TRAJECTORY = 16
00111 };      
00112 
00113 typedef struct{
00114   bool accept_partial_plans;
00115   bool accept_invalid_goals;
00116   bool disable_ik;
00117   bool disable_collision_monitoring;
00118   bool is_pose_goal;
00119   double allowed_planning_time;
00120   std::string planner_service_name;
00121 } MoveArmParameters;
00122   
00123 static const std::string ARM_IK_NAME = "arm_ik";
00124 static const std::string TRAJECTORY_FILTER = "/trajectory_filter_server/filter_trajectory_with_constraints";
00125 static const std::string DISPLAY_PATH_PUB_TOPIC  = "display_path";
00126 static const std::string DISPLAY_JOINT_GOAL_PUB_TOPIC  = "display_joint_goal";
00127 
00128 //bunch of statics for remapping purposes
00129 
00130 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
00131 static const double MIN_TRAJECTORY_MONITORING_FREQUENCY = 1.0;
00132 static const double MAX_TRAJECTORY_MONITORING_FREQUENCY = 100.0;
00133   
00134 class MoveArm
00135 {
00136 public: 
00137 
00138   MoveArm(const std::string &group_name) :  
00139     group_(group_name), 
00140     private_handle_("~")
00141   {
00142     private_handle_.param<double>("move_arm_frequency",move_arm_frequency_, 50.0);
00143     private_handle_.param<double>("trajectory_filter_allowed_time",trajectory_filter_allowed_time_, 2.0);
00144     private_handle_.param<double>("ik_allowed_time",ik_allowed_time_, 2.0);
00145 
00146     private_handle_.param<bool>("publish_stats",publish_stats_, true);
00147 
00148     planning_scene_state_ = NULL;
00149 
00150     collision_models_ = new planning_environment::CollisionModels("robot_description");
00151 
00152     num_planning_attempts_ = 0;
00153     state_ = PLANNING;
00154 
00155     ik_client_ = root_handle_.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK>(ARM_IK_NAME);
00156     allowed_contact_regions_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("allowed_contact_regions_array", 128);
00157     filter_trajectory_client_ = root_handle_.serviceClient<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>(TRAJECTORY_FILTER);      
00158     vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("move_" + group_name+"_markers", 128);
00159     vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("move_" + group_name+"_markers_array", 128);
00160     get_state_client_ = root_handle_.serviceClient<arm_navigation_msgs::GetRobotState>("/environment_server/get_robot_state");      
00161 
00162     //    ros::service::waitForService(ARM_IK_NAME);
00163     arm_ik_initialized_ = false;
00164     ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
00165     set_planning_scene_diff_client_ = root_handle_.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);
00166     
00167     ros::service::waitForService(TRAJECTORY_FILTER);
00168 
00169     action_server_.reset(new actionlib::SimpleActionServer<arm_navigation_msgs::MoveArmAction>(root_handle_, "move_" + group_name, boost::bind(&MoveArm::execute, this, _1), false));
00170     action_server_->start();
00171 
00172     display_path_publisher_ = root_handle_.advertise<arm_navigation_msgs::DisplayTrajectory>(DISPLAY_PATH_PUB_TOPIC, 1, true);
00173     display_joint_goal_publisher_ = root_handle_.advertise<arm_navigation_msgs::DisplayTrajectory>(DISPLAY_JOINT_GOAL_PUB_TOPIC, 1, true);
00174     stats_publisher_ = private_handle_.advertise<arm_navigation_msgs::MoveArmStatistics>("statistics",1,true);
00175   }     
00176   virtual ~MoveArm()
00177   {
00178     revertPlanningScene();
00179     delete collision_models_;
00180   }
00181 
00182   bool configure()
00183   {
00184     if(!initializeControllerInterface())
00185     {
00186       ROS_ERROR("Could not initialize controller interface");
00187       return false;
00188     }
00189     if (group_.empty())
00190     {
00191       ROS_ERROR("No 'group' parameter specified. Without the name of the group of joints to plan for, action cannot start");
00192       return false;
00193     }
00194     const planning_models::KinematicModel::JointModelGroup* joint_model_group = collision_models_->getKinematicModel()->getModelGroup(group_);
00195     if(joint_model_group == NULL) {
00196       ROS_WARN_STREAM("No joint group " << group_);
00197       return false;
00198     }
00199     group_joint_names_ = joint_model_group->getJointModelNames();
00200     group_link_names_ = joint_model_group->getGroupLinkNames();
00201     return true;
00202   }
00203         
00204 private:
00205 
00209   bool convertPoseGoalToJointGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
00210   {
00211     if(!arm_ik_initialized_)
00212     {
00213       if(!ros::service::waitForService(ARM_IK_NAME,ros::Duration(1.0)))
00214       {
00215         ROS_WARN("Inverse kinematics service is unavailable");
00216         return false;
00217       }
00218       else
00219       {
00220         arm_ik_initialized_ = true;
00221       }
00222     }
00223 
00224 
00225     ROS_DEBUG("Acting on goal to pose ...");// we do IK to find corresponding states
00226     ROS_DEBUG("Position constraint: %f %f %f",
00227               req.motion_plan_request.goal_constraints.position_constraints[0].position.x,
00228               req.motion_plan_request.goal_constraints.position_constraints[0].position.y,
00229               req.motion_plan_request.goal_constraints.position_constraints[0].position.z);
00230     ROS_DEBUG("Orientation constraint: %f %f %f %f",
00231               req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x,
00232               req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y,
00233               req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z,
00234               req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w);
00235 
00236     geometry_msgs::PoseStamped tpose = arm_navigation_msgs::poseConstraintsToPoseStamped(req.motion_plan_request.goal_constraints.position_constraints[0],
00237                                                                                           req.motion_plan_request.goal_constraints.orientation_constraints[0]);
00238     std::string link_name = req.motion_plan_request.goal_constraints.position_constraints[0].link_name;
00239     sensor_msgs::JointState solution;           
00240     
00241     ROS_DEBUG("IK request");
00242     ROS_DEBUG("link_name   : %s",link_name.c_str());
00243     ROS_DEBUG("frame_id    : %s",tpose.header.frame_id.c_str());
00244     ROS_DEBUG("position    : (%f,%f,%f)",tpose.pose.position.x,tpose.pose.position.y,tpose.pose.position.z);
00245     ROS_DEBUG("orientation : (%f,%f,%f,%f)",tpose.pose.orientation.x,tpose.pose.orientation.y,tpose.pose.orientation.z,tpose.pose.orientation.w);
00246     ROS_DEBUG(" ");
00247     if (computeIK(tpose, 
00248                   link_name, 
00249                   solution))
00250     {
00251       std::map<std::string, double> joint_values;
00252       for (unsigned int i = 0 ; i < solution.name.size() ; ++i)
00253       {
00254         arm_navigation_msgs::JointConstraint jc;
00255         jc.joint_name = solution.name[i];
00256         jc.position = solution.position[i];
00257         jc.tolerance_below = 0.01;
00258         jc.tolerance_above = 0.01;
00259         req.motion_plan_request.goal_constraints.joint_constraints.push_back(jc);
00260         joint_values[jc.joint_name] = jc.position;
00261       }
00262       arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00263       resetToStartState(planning_scene_state_);
00264       planning_scene_state_->setKinematicState(joint_values);
00265       if(!collision_models_->isKinematicStateValid(*planning_scene_state_,
00266                                                   group_joint_names_,
00267                                                   error_code,
00268                                                   original_request_.motion_plan_request.goal_constraints,
00269                                                    original_request_.motion_plan_request.path_constraints,
00270                                                    true))
00271       {
00272         ROS_INFO("IK returned joint state for goal that doesn't seem to be valid");
00273         if(error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) {
00274           ROS_WARN("IK solution doesn't obey goal constraints");
00275         } else if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00276           ROS_WARN("IK solution in collision");
00277         } else {
00278           ROS_WARN_STREAM("Some other problem with ik solution " << error_code.val);
00279         }
00280       }
00281       req.motion_plan_request.goal_constraints.position_constraints.clear();
00282       req.motion_plan_request.goal_constraints.orientation_constraints.clear();     
00283       return true;
00284     }
00285     else
00286       return false;
00287   }
00288 
00289   bool computeIK(const geometry_msgs::PoseStamped &pose_stamped_msg,  
00290                  const std::string &link_name, 
00291                  sensor_msgs::JointState &solution)
00292   {
00293     kinematics_msgs::GetConstraintAwarePositionIK::Request request;
00294     kinematics_msgs::GetConstraintAwarePositionIK::Response response;
00295             
00296     request.ik_request.pose_stamped = pose_stamped_msg;
00297     request.ik_request.robot_state = original_request_.motion_plan_request.start_state;
00298     request.ik_request.ik_seed_state = request.ik_request.robot_state;
00299     request.ik_request.ik_link_name = link_name;
00300     request.timeout = ros::Duration(ik_allowed_time_);
00301     request.constraints = original_request_.motion_plan_request.goal_constraints;
00302     if (ik_client_.call(request, response))
00303     {
00304       move_arm_action_result_.error_code = response.error_code;
00305       if(response.error_code.val != response.error_code.SUCCESS)
00306       {
00307         ROS_ERROR("IK Solution not found, IK returned with error_code: %d",response.error_code.val);
00308         return false;
00309       }         
00310       solution = response.solution.joint_state;
00311       if (solution.position.size() != group_joint_names_.size())
00312       {
00313         ROS_ERROR("Incorrect number of elements in IK output.");
00314         return false;
00315       }
00316       for(unsigned int i = 0; i < solution.position.size() ; ++i)
00317         ROS_DEBUG("IK[%d] = %f", (int)i, solution.position[i]);
00318     }
00319     else
00320     {
00321       ROS_ERROR("IK service failed");
00322       return false;
00323     }       
00324     return true;
00325   }
00329 
00333   bool filterTrajectory(const trajectory_msgs::JointTrajectory &trajectory_in, 
00334                         trajectory_msgs::JointTrajectory &trajectory_out)
00335   {
00336     arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request  req;
00337     arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response res;
00338     fillTrajectoryMsg(trajectory_in, req.trajectory);
00339 
00340     if(trajectory_filter_allowed_time_ == 0.0)
00341     {
00342       trajectory_out = req.trajectory;
00343       return true;
00344     }
00345     resetToStartState(planning_scene_state_);
00346     planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
00347                                                             ros::Time::now(),
00348                                                             collision_models_->getWorldFrameId(),
00349                                                             req.start_state);
00350     req.group_name = group_;
00351     req.path_constraints = original_request_.motion_plan_request.path_constraints;
00352     req.goal_constraints = original_request_.motion_plan_request.goal_constraints;
00353     req.allowed_time = ros::Duration(trajectory_filter_allowed_time_);
00354     ros::Time smoothing_time = ros::Time::now();
00355     if(filter_trajectory_client_.call(req,res))
00356     {
00357       move_arm_stats_.trajectory_duration = (res.trajectory.points.back().time_from_start-res.trajectory.points.front().time_from_start).toSec();
00358       move_arm_stats_.smoothing_time = (ros::Time::now()-smoothing_time).toSec();
00359       trajectory_out = res.trajectory;
00360       return true;
00361     }
00362     else
00363     {
00364       ROS_ERROR("Service call to filter trajectory failed.");
00365       return false;
00366     }
00367   }
00368 
00372  
00373   void discretizeTrajectory(const trajectory_msgs::JointTrajectory &trajectory, 
00374                             trajectory_msgs::JointTrajectory &trajectory_out,
00375                             const double &trajectory_discretization)
00376   {    
00377     trajectory_out.joint_names = trajectory.joint_names;
00378     for(unsigned int i=1; i < trajectory.points.size(); i++)
00379     {
00380       double diff = 0.0;      
00381       for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++)
00382       {
00383         double start = trajectory.points[i-1].positions[j];
00384         double end   = trajectory.points[i].positions[j];
00385         if(fabs(end-start) > diff)
00386           diff = fabs(end-start);        
00387       }
00388       int num_intervals =(int) (diff/trajectory_discretization+1.0);
00389       
00390       for(unsigned int k=0; k < (unsigned int) num_intervals; k++)
00391       {
00392         trajectory_msgs::JointTrajectoryPoint point;
00393         for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++)
00394         {
00395           double start = trajectory.points[i-1].positions[j];
00396           double end   = trajectory.points[i].positions[j];
00397           point.positions.push_back(start + (end-start)*k/num_intervals);
00398         }
00399         point.time_from_start = ros::Duration(trajectory.points[i].time_from_start.toSec() + k* (trajectory.points[i].time_from_start - trajectory.points[i-1].time_from_start).toSec()/num_intervals);
00400         trajectory_out.points.push_back(point);
00401       }
00402     }
00403     trajectory_out.points.push_back(trajectory.points.back());
00404   }
00408 
00412   bool isPoseGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
00413   {
00414     if (req.motion_plan_request.goal_constraints.joint_constraints.empty() &&         // we have no joint constraints on the goal,
00415         req.motion_plan_request.goal_constraints.position_constraints.size() == 1 &&      // we have a single position constraint on the goal
00416         req.motion_plan_request.goal_constraints.orientation_constraints.size() ==  1)  // that is active on all 6 DOFs
00417       return true;
00418     else
00419       return false;
00420   }       
00421   bool hasPoseGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
00422   {
00423     if (req.motion_plan_request.goal_constraints.position_constraints.size() >= 1 &&      // we have a single position constraint on the goal
00424         req.motion_plan_request.goal_constraints.orientation_constraints.size() >=  1)  // that is active on all 6 DOFs
00425       return true;
00426     else
00427       return false;
00428   }       
00429   bool isJointGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
00430   {
00431     if (req.motion_plan_request.goal_constraints.position_constraints.empty() && 
00432         req.motion_plan_request.goal_constraints.orientation_constraints.empty() && 
00433         !req.motion_plan_request.goal_constraints.joint_constraints.empty())
00434       return true;
00435     else
00436       return false;
00437   }                   
00438 
00439   //stubbing out for now
00440   bool isExecutionSafe() {
00441     return true;
00442   }
00443 
00444   bool getRobotState(planning_models::KinematicState* state)
00445   {
00446     arm_navigation_msgs::GetRobotState::Request req;
00447     arm_navigation_msgs::GetRobotState::Response res;
00448     if(get_state_client_.call(req,res))
00449     {
00450       planning_environment::setRobotStateAndComputeTransforms(res.robot_state, *state);
00451     }
00452     else
00453     {
00454       ROS_ERROR("Service call to get robot state failed on %s",
00455                 get_state_client_.getService().c_str());
00456       return false;
00457     }
00458     return true;
00459   }
00463 
00467   void moveArmGoalToPlannerRequest(const arm_navigation_msgs::MoveArmGoalConstPtr& goal, 
00468                                    arm_navigation_msgs::GetMotionPlan::Request &req)
00469   {
00470     req.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp = ros::Time::now();
00471     req.motion_plan_request = goal->motion_plan_request;
00472 
00473     move_arm_parameters_.accept_partial_plans = goal->accept_partial_plans;
00474     move_arm_parameters_.accept_invalid_goals = goal->accept_invalid_goals;
00475     move_arm_parameters_.disable_ik           = goal->disable_ik;
00476     move_arm_parameters_.disable_collision_monitoring = goal->disable_collision_monitoring;
00477     move_arm_parameters_.allowed_planning_time = goal->motion_plan_request.allowed_planning_time.toSec();
00478     move_arm_parameters_.planner_service_name = goal->planner_service_name;
00479     // visualizeAllowedContactRegions(req.motion_plan_request.allowed_contacts);
00480     // ROS_INFO("Move arm: %d allowed contact regions",(int)req.motion_plan_request.allowed_contacts.size());
00481     // for(unsigned int i=0; i < req.motion_plan_request.allowed_contacts.size(); i++)
00482     // {
00483     //   ROS_INFO("Position                    : (%f,%f,%f)",req.motion_plan_request.allowed_contacts[i].pose_stamped.pose.position.x,req.motion_plan_request.allowed_contacts[i].pose_stamped.pose.position.y,req.motion_plan_request.allowed_contacts[i].pose_stamped.pose.position.z);
00484     //   ROS_INFO("Frame id                    : %s",req.motion_plan_request.allowed_contacts[i].pose_stamped.header.frame_id.c_str());
00485     //   ROS_INFO("Depth                       : %f",req.motion_plan_request.allowed_contacts[i].penetration_depth);
00486     //   ROS_INFO("Link                        : %s",req.motion_plan_request.allowed_contacts[i].link_names[0].c_str());
00487     //   ROS_INFO(" ");
00488     // }
00489   }
00490   bool doPrePlanningChecks(arm_navigation_msgs::GetMotionPlan::Request &req,  
00491                            arm_navigation_msgs::GetMotionPlan::Response &res)
00492   {
00493     arm_navigation_msgs::Constraints empty_goal_constraints;
00494     if(planning_scene_state_ == NULL) {
00495       ROS_INFO("Can't do pre-planning checks without planning state");
00496     }
00497     resetToStartState(planning_scene_state_);
00498     arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00499     if(!collision_models_->isKinematicStateValid(*planning_scene_state_,
00500                                                  group_joint_names_,
00501                                                  error_code,
00502                                                  empty_goal_constraints,
00503                                                  original_request_.motion_plan_request.path_constraints,
00504                                                  true)) {
00505       if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00506         move_arm_action_result_.error_code.val = error_code.START_STATE_IN_COLLISION;
00507         collision_models_->getAllCollisionsForState(*planning_scene_state_,
00508                                                     move_arm_action_result_.contacts);
00509         ROS_ERROR("Starting state is in collision, can't plan");
00510         visualization_msgs::MarkerArray arr;
00511         std_msgs::ColorRGBA point_color_;
00512         point_color_.a = 1.0;
00513         point_color_.r = 1.0;
00514         point_color_.g = .8;
00515         point_color_.b = 0.04;
00516 
00517         collision_models_->getAllCollisionPointMarkers(*planning_scene_state_,
00518                                                        arr,
00519                                                        point_color_,
00520                                                        ros::Duration(0.0)); 
00521         std_msgs::ColorRGBA col;
00522         col.a = .9;
00523         col.r = 1.0;
00524         col.b = 1.0;
00525         col.g = 0.0;
00526         /*
00527           collision_models_->getRobotTrimeshMarkersGivenState(*planning_scene_state_,
00528           arr,
00529           true,
00530           ros::Duration(0.0));
00531         */
00532         vis_marker_array_publisher_.publish(arr);
00533       } else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) {
00534         move_arm_action_result_.error_code.val = error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS;
00535         ROS_ERROR("Starting state violated path constraints, can't plan");;
00536       } else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED) {
00537         move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.JOINT_LIMITS_VIOLATED;;
00538         ROS_ERROR("Start state violates joint limits, can't plan.");
00539       }
00540       ROS_INFO("Setting aborted because start state invalid");
00541       action_server_->setAborted(move_arm_action_result_);
00542       return false;
00543     }
00544     // processing and checking goal
00545     if (!move_arm_parameters_.disable_ik && isPoseGoal(req)) {
00546       ROS_DEBUG("Planning to a pose goal");
00547       if(!convertPoseGoalToJointGoal(req)) {
00548         ROS_INFO("Setting aborted because ik failed");
00549         action_server_->setAborted(move_arm_action_result_);
00550         return false;
00551       }
00552     }
00553     //if we still have pose constraints at this point it's probably a constrained combo goal
00554     if(!hasPoseGoal(req)) {
00555       arm_navigation_msgs::RobotState empty_state;
00556       empty_state.joint_state = arm_navigation_msgs::jointConstraintsToJointState(req.motion_plan_request.goal_constraints.joint_constraints);
00557       planning_environment::setRobotStateAndComputeTransforms(empty_state, *planning_scene_state_);
00558       arm_navigation_msgs::Constraints empty_constraints;
00559       if(!collision_models_->isKinematicStateValid(*planning_scene_state_,
00560                                                    group_joint_names_,
00561                                                    error_code,
00562                                                    original_request_.motion_plan_request.goal_constraints,
00563                                                    original_request_.motion_plan_request.path_constraints,
00564                                                    true)) {
00565         if(error_code.val == error_code.JOINT_LIMITS_VIOLATED) {
00566           ROS_ERROR("Will not plan to requested joint goal since it violates joint limits constraints");
00567           move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.JOINT_LIMITS_VIOLATED;
00568         } else if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00569           ROS_ERROR("Will not plan to requested joint goal since it is in collision");
00570           move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.GOAL_IN_COLLISION;
00571           collision_models_->getAllCollisionsForState(*planning_scene_state_,
00572                                                       move_arm_action_result_.contacts);
00573         } else if(error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) {
00574           ROS_ERROR("Will not plan to requested joint goal since it violates goal constraints");
00575           move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS;
00576         } else if(error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) {
00577           ROS_ERROR("Will not plan to requested joint goal since it violates path constraints");
00578           move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS;
00579         } else {
00580           ROS_INFO_STREAM("Will not plan to request joint goal due to error code " << error_code.val);
00581         }
00582         ROS_INFO_STREAM("Setting aborted becuase joint goal is problematic");
00583         action_server_->setAborted(move_arm_action_result_);
00584         return false;
00585       }
00586     }
00587     return true;
00588   }
00589 
00590   bool createPlan(arm_navigation_msgs::GetMotionPlan::Request &req,  
00591                   arm_navigation_msgs::GetMotionPlan::Response &res)
00592   {
00593     while(!ros::service::waitForService(move_arm_parameters_.planner_service_name, ros::Duration(1.0))) {
00594       ROS_INFO_STREAM("Waiting for requested service " << move_arm_parameters_.planner_service_name);
00595     }
00596     ros::ServiceClient planning_client = root_handle_.serviceClient<arm_navigation_msgs::GetMotionPlan>(move_arm_parameters_.planner_service_name);
00597     move_arm_stats_.planner_service_name = move_arm_parameters_.planner_service_name;
00598     ROS_DEBUG("Issuing request for motion plan");                   
00599     // call the planner and decide whether to use the path
00600     if (planning_client.call(req, res))
00601     {
00602       if (res.trajectory.joint_trajectory.points.empty())
00603       {
00604         ROS_WARN("Motion planner was unable to plan a path to goal");
00605         return false;
00606       }
00607       ROS_DEBUG("Motion planning succeeded");
00608       return true;
00609     }
00610     else
00611     {
00612       ROS_ERROR("Motion planning service failed on %s",planning_client.getService().c_str());
00613       return false;
00614     }
00615   }
00619 
00623   bool initializeControllerInterface()
00624   {
00625     std::string controller_action_name;
00626     private_handle_.param<std::string>("controller_action_name", controller_action_name, "action");
00627     ROS_INFO("Connecting to controller using action: %s",controller_action_name.c_str());
00628     controller_action_client_ = new JointExecutorActionClient(controller_action_name);
00629     if(!controller_action_client_) {
00630       ROS_ERROR("Controller action client hasn't been initialized yet");
00631       return false;
00632     }
00633     while(!controller_action_client_->waitForActionServerToStart(ros::Duration(1.0))){
00634       ROS_INFO("Waiting for the joint_trajectory_action server to come up.");
00635       if(!root_handle_.ok()) {
00636         return false;
00637       }
00638     }
00639     ROS_INFO("Connected to the controller");
00640     return true;
00641   }
00642 
00643 
00644   bool stopTrajectory()
00645   {
00646     if (controller_goal_handle_.isExpired())
00647       ROS_ERROR("Expired goal handle.  controller_status = %d", controller_status_);
00648     else
00649       controller_goal_handle_.cancel();
00650     return true;
00651   }
00652   bool sendTrajectory(trajectory_msgs::JointTrajectory &current_trajectory)
00653   {
00654     current_trajectory.header.stamp = ros::Time::now()+ros::Duration(0.2);
00655 
00656     control_msgs::FollowJointTrajectoryGoal goal;  
00657     goal.trajectory = current_trajectory;
00658 
00659     ROS_DEBUG("Sending trajectory with %d points and timestamp: %f",(int)goal.trajectory.points.size(),goal.trajectory.header.stamp.toSec());
00660     for(unsigned int i=0; i < goal.trajectory.joint_names.size(); i++)
00661       ROS_DEBUG("Joint: %d name: %s",i,goal.trajectory.joint_names[i].c_str());
00662 
00663     /*    for(unsigned int i = 0; i < goal.trajectory.points.size(); i++)
00664       {
00665         ROS_INFO("%f: %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
00666                   goal.trajectory.points[i].time_from_start.toSec(),
00667                   goal.trajectory.points[i].positions[0],
00668                   goal.trajectory.points[i].positions[1],
00669                   goal.trajectory.points[i].positions[2],
00670                   goal.trajectory.points[i].positions[3],
00671                   goal.trajectory.points[i].positions[4],
00672                   goal.trajectory.points[i].positions[5],
00673                   goal.trajectory.points[i].positions[6],
00674                   goal.trajectory.points[i].velocities[0],
00675                   goal.trajectory.points[i].velocities[1],
00676                   goal.trajectory.points[i].velocities[2],
00677                   goal.trajectory.points[i].velocities[3],
00678                   goal.trajectory.points[i].velocities[4],
00679                   goal.trajectory.points[i].velocities[5],
00680                   goal.trajectory.points[i].velocities[6]);
00681                   }*/
00682     controller_goal_handle_ = controller_action_client_->sendGoal(goal,boost::bind(&MoveArm::controllerTransitionCallback, this, _1));
00683 
00684     controller_status_ = QUEUED;
00685     //    printTrajectory(goal.trajectory);
00686     return true;
00687   }
00688 
00689   void controllerTransitionCallback(JointExecutorActionClient::GoalHandle gh) 
00690   {   
00691     if(gh != controller_goal_handle_)
00692       return;
00693     actionlib::CommState comm_state = gh.getCommState();    
00694     switch( comm_state.state_)
00695     {
00696     case actionlib::CommState::WAITING_FOR_GOAL_ACK:
00697     case actionlib::CommState::PENDING:
00698     case actionlib::CommState::RECALLING:
00699       controller_status_ = QUEUED;
00700       return;
00701     case actionlib:: CommState::ACTIVE:
00702     case actionlib::CommState::PREEMPTING:
00703       controller_status_ = ACTIVE;
00704       return;
00705     case actionlib::CommState::DONE:
00706       {
00707         switch(gh.getTerminalState().state_)
00708         {
00709         case actionlib::TerminalState::RECALLED:
00710         case actionlib::TerminalState::REJECTED:
00711         case actionlib::TerminalState::PREEMPTED:
00712         case actionlib::TerminalState::ABORTED:
00713         case actionlib::TerminalState::LOST:
00714           {
00715             ROS_INFO("Trajectory controller status came back as failed");
00716             controller_status_ = FAILED;
00717             controller_goal_handle_.reset();
00718             return;
00719           }
00720         case actionlib::TerminalState::SUCCEEDED:
00721           {
00722             controller_goal_handle_.reset();
00723             controller_status_ = SUCCESS;         
00724             return;
00725           }
00726         default:
00727           ROS_ERROR("Unknown terminal state [%u]. This is a bug in ActionClient", gh.getTerminalState().state_);
00728         }
00729       }
00730     default:
00731       break;
00732     }
00733   } 
00734   bool isControllerDone(arm_navigation_msgs::ArmNavigationErrorCodes& error_code)
00735   {      
00736     if (controller_status_ == SUCCESS)
00737     {
00738       error_code.val = error_code.SUCCESS;
00739       return true;
00740     } else if(controller_status_ == FAILED)
00741     {
00742       error_code.val = error_code.TRAJECTORY_CONTROLLER_FAILED;
00743       return true;
00744     } else {
00745       return false;
00746     }
00747   }
00748 
00749   void fillTrajectoryMsg(const trajectory_msgs::JointTrajectory &trajectory_in, 
00750                          trajectory_msgs::JointTrajectory &trajectory_out)
00751   {
00752     trajectory_out = trajectory_in;
00753     if(trajectory_in.points.empty())
00754     {
00755       ROS_WARN("No points in trajectory");
00756       return;
00757     }
00758     // get the current state
00759     double d = 0.0;
00760 
00761 
00762     std::map<std::string, double> val_map;
00763     resetToStartState(planning_scene_state_);
00764     planning_scene_state_->getKinematicStateValues(val_map);
00765     sensor_msgs::JointState current;
00766     current.name = trajectory_out.joint_names;
00767     current.position.resize(trajectory_out.joint_names.size());
00768     for(unsigned int i = 0; i < trajectory_out.joint_names.size(); i++) {
00769       current.position[i] = val_map[trajectory_out.joint_names[i]];
00770     }
00771     std::map<std::string, bool> continuous;
00772     for(unsigned int j = 0; j < trajectory_in.joint_names.size(); j++) {
00773       std::string name = trajectory_in.joint_names[j];
00774       boost::shared_ptr<const urdf::Joint> joint = collision_models_->getParsedDescription()->getJoint(name);
00775       if (joint.get() == NULL)
00776       {
00777         ROS_ERROR("Joint name %s not found in urdf model", name.c_str());
00778         return;
00779       }
00780       if (joint->type == urdf::Joint::CONTINUOUS) {
00781         continuous[name] = true;
00782       } else {
00783         continuous[name] = false;
00784       }
00785     }
00786     for (unsigned int i = 0 ; i < current.position.size() ; ++i)
00787     {
00788       double diff; 
00789       if(!continuous[trajectory_in.joint_names[i]]) {
00790         diff = fabs(trajectory_in.points[0].positions[i] - val_map[trajectory_in.joint_names[i]]);
00791       } else {
00792         diff = angles::shortest_angular_distance(trajectory_in.points[0].positions[i],val_map[trajectory_in.joint_names[i]]);
00793       }
00794       d += diff * diff;
00795     }
00796     d = sqrt(d);            
00797     // decide whether we place the current state in front of the trajectory_in
00798     int include_first = (d > 0.1) ? 1 : 0;
00799     double offset = 0.0;
00800     trajectory_out.points.resize(trajectory_in.points.size() + include_first);
00801 
00802     if (include_first)
00803     {
00804       ROS_INFO("Adding current state to front of trajectory");
00805       // ROS_INFO_STREAM("Old state " << trajectory_out.points[0].positions[0]
00806       //                      << trajectory_out.points[0].positions[1]
00807       //                      << trajectory_out.points[0].positions[2]
00808       //                      << trajectory_out.points[0].positions[3]
00809       //                      << trajectory_out.points[0].positions[4]
00810       //                      << trajectory_out.points[0].positions[5]
00811       //                      << trajectory_out.points[0].positions[6]);
00812 
00813       // ROS_INFO_STREAM("Current state " << current.position[0]
00814       //                      << current.position[1]
00815       //                      << current.position[2]
00816       //                      << current.position[3]
00817       //                      << current.position[4]
00818       //                      << current.position[5]
00819       //                      << current.position[6]);
00820       trajectory_out.points[0].positions = arm_navigation_msgs::jointStateToJointTrajectoryPoint(current).positions;
00821       trajectory_out.points[0].time_from_start = ros::Duration(0.0);
00822       offset = 0.3 + d;
00823     } 
00824     for (unsigned int i = 0 ; i < trajectory_in.points.size() ; ++i)
00825     {
00826       trajectory_out.points[i+include_first].time_from_start = trajectory_in.points[i].time_from_start;
00827       trajectory_out.points[i+include_first].positions = trajectory_in.points[i].positions;
00828     }
00829     trajectory_out.header.stamp = ros::Time::now();
00830   }
00831 
00835 
00839   void resetStateMachine()
00840   {
00841     num_planning_attempts_ = 0;
00842     current_trajectory_.points.clear();
00843     current_trajectory_.joint_names.clear();
00844     state_ = PLANNING;    
00845   }
00846   bool executeCycle(arm_navigation_msgs::GetMotionPlan::Request &req)
00847   {
00848     arm_navigation_msgs::GetMotionPlan::Response res;
00849     arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00850     
00851     switch(state_)
00852     {
00853     case PLANNING:
00854       {
00855         move_arm_action_feedback_.state = "planning";
00856         move_arm_action_feedback_.time_to_completion = ros::Duration(req.motion_plan_request.allowed_planning_time);
00857         action_server_->publishFeedback(move_arm_action_feedback_);
00858 
00859         if(!doPrePlanningChecks(req,res))
00860           return true;
00861 
00862         visualizeJointGoal(req);
00863         resetToStartState(planning_scene_state_);
00864         if(collision_models_->isKinematicStateValid(*planning_scene_state_,
00865                                                     group_joint_names_,
00866                                                     error_code,
00867                                                     original_request_.motion_plan_request.goal_constraints,
00868                                                     original_request_.motion_plan_request.path_constraints,
00869                                                     false)) {
00870           resetStateMachine();
00871           move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.SUCCESS;
00872           action_server_->setSucceeded(move_arm_action_result_);
00873           ROS_INFO("Apparently start state satisfies goal");
00874           return true;
00875         }
00876         ros::Time planning_time = ros::Time::now();
00877         if(createPlan(req,res))
00878         {
00879           std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> traj_error_codes;
00880           move_arm_stats_.planning_time = (ros::Time::now()-planning_time).toSec();
00881           ROS_DEBUG("createPlan succeeded");
00882           resetToStartState(planning_scene_state_);
00883           if(!collision_models_->isJointTrajectoryValid(*planning_scene_state_,
00884                                                         res.trajectory.joint_trajectory, 
00885                                                         original_request_.motion_plan_request.goal_constraints,
00886                                                         original_request_.motion_plan_request.path_constraints,
00887                                                         error_code,
00888                                                         traj_error_codes,
00889                                                         true))
00890           {
00891             if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00892               ROS_WARN("Planner trajectory collides");
00893             } else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) {
00894               ROS_WARN("Planner trajectory violates path constraints");
00895             } else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED) {
00896               ROS_WARN("Planner trajectory violates joint limits");
00897             } else if (error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) {
00898               ROS_WARN("Planner trajectory doesn't reach goal");
00899             }
00900             num_planning_attempts_++;
00901             if(num_planning_attempts_ > req.motion_plan_request.num_planning_attempts)
00902             {
00903               resetStateMachine();
00904               ROS_INFO_STREAM("Setting aborted because we're out of planning attempts");
00905               action_server_->setAborted(move_arm_action_result_);
00906               return true;
00907             }
00908           }
00909           else{
00910             ROS_DEBUG("Trajectory validity check was successful");
00911             
00912             current_trajectory_ = res.trajectory.joint_trajectory;
00913             visualizePlan(current_trajectory_);
00914             //          printTrajectory(current_trajectory_);
00915             state_ = START_CONTROL;
00916             ROS_DEBUG("Done planning. Transitioning to control");
00917           }
00918         }
00919         else if(action_server_->isActive())
00920         {
00921           num_planning_attempts_++;
00922           arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00923           error_code.val = error_code.PLANNING_FAILED;
00924           if(num_planning_attempts_ > req.motion_plan_request.num_planning_attempts)
00925           {
00926             resetStateMachine();
00927             ROS_INFO_STREAM("Setting aborted because we're out of planning attempts");
00928             action_server_->setAborted(move_arm_action_result_);
00929             return true;
00930           }
00931         }
00932         else
00933         {
00934           ROS_ERROR("create plan failed");
00935         }
00936         break;
00937       }
00938     case START_CONTROL:
00939       {
00940         move_arm_action_feedback_.state = "start_control";
00941         move_arm_action_feedback_.time_to_completion = ros::Duration(1.0/move_arm_frequency_);
00942         action_server_->publishFeedback(move_arm_action_feedback_);
00943         ROS_DEBUG("Filtering Trajectory");
00944         trajectory_msgs::JointTrajectory filtered_trajectory;
00945         if(filterTrajectory(current_trajectory_, filtered_trajectory))
00946         {
00947           arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00948           std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> traj_error_codes;
00949           resetToStartState(planning_scene_state_);
00950           if(!collision_models_->isJointTrajectoryValid(*planning_scene_state_,
00951                                                         filtered_trajectory,
00952                                                         original_request_.motion_plan_request.goal_constraints,
00953                                                         original_request_.motion_plan_request.path_constraints,
00954                                                         error_code,
00955                                                         traj_error_codes,
00956                                                         false))
00957           {
00958             if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00959               ROS_WARN("Filtered trajectory collides");
00960             } else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) {
00961               ROS_WARN("Filtered trajectory violates path constraints");
00962             } else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED) {
00963               ROS_WARN("Filtered trajectory violates joint limits");
00964             } else if (error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) {
00965               ROS_WARN("Filtered trajectory doesn't reach goal");
00966             }
00967             ROS_ERROR("Move arm will abort this goal.  Will replan");
00968             state_ = PLANNING;
00969             num_planning_attempts_++;       
00970             if(num_planning_attempts_ > req.motion_plan_request.num_planning_attempts)
00971             {
00972               resetStateMachine();
00973               ROS_INFO_STREAM("Setting aborted because we're out of planning attempts");
00974               action_server_->setAborted(move_arm_action_result_);
00975               return true;
00976             }
00977             //resetStateMachine();
00978             //action_server_->setAborted(move_arm_action_result_);
00979             break;
00980             //return true;
00981           }
00982           else{
00983             ROS_DEBUG("Trajectory validity check was successful");
00984           }
00985           current_trajectory_ = filtered_trajectory;
00986         } else {
00987           resetStateMachine();
00988           ROS_INFO_STREAM("Setting aborted because trajectory filter call failed");
00989           action_server_->setAborted(move_arm_action_result_);
00990           return true;              
00991         }
00992         ROS_DEBUG("Sending trajectory");
00993         move_arm_stats_.time_to_execution = (ros::Time::now() - ros::Time(move_arm_stats_.time_to_execution)).toSec();
00994         if(sendTrajectory(current_trajectory_))
00995         {
00996           state_ = MONITOR;
00997         }
00998         else
00999         {
01000           resetStateMachine();
01001           ROS_INFO("Setting aborted because we couldn't send the trajectory");
01002           action_server_->setAborted(move_arm_action_result_);
01003           return true;              
01004         }
01005         break;
01006       }
01007     case MONITOR:
01008       {
01009         move_arm_action_feedback_.state = "monitor";
01010         move_arm_action_feedback_.time_to_completion = current_trajectory_.points.back().time_from_start;
01011         action_server_->publishFeedback(move_arm_action_feedback_);
01012         ROS_DEBUG("Start to monitor");
01013         arm_navigation_msgs::ArmNavigationErrorCodes controller_error_code;
01014         if(isControllerDone(controller_error_code))
01015         {
01016           move_arm_stats_.time_to_result = (ros::Time::now()-ros::Time(move_arm_stats_.time_to_result)).toSec();
01017 
01018           arm_navigation_msgs::RobotState empty_state;
01019           arm_navigation_msgs::ArmNavigationErrorCodes state_error_code;
01020           getRobotState(planning_scene_state_);
01021           if(collision_models_->isKinematicStateValid(*planning_scene_state_,
01022                                                       group_joint_names_,
01023                                                       state_error_code,
01024                                                       original_request_.motion_plan_request.goal_constraints,
01025                                                       original_request_.motion_plan_request.path_constraints,
01026                                                       true))
01027           {
01028             move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.SUCCESS;
01029             resetStateMachine();
01030             action_server_->setSucceeded(move_arm_action_result_);
01031             if(controller_error_code.val == controller_error_code.TRAJECTORY_CONTROLLER_FAILED) {
01032               ROS_INFO("Trajectory controller failed but we seem to be at goal");
01033             } else {
01034               ROS_DEBUG("Reached goal");
01035             }
01036             return true;
01037           }
01038           else
01039           {
01040             if(state_error_code.val == state_error_code.COLLISION_CONSTRAINTS_VIOLATED) {
01041               move_arm_action_result_.error_code.val = state_error_code.START_STATE_IN_COLLISION;
01042               collision_models_->getAllCollisionsForState(*planning_scene_state_,
01043                                                           move_arm_action_result_.contacts);
01044               ROS_WARN("Though trajectory is done current state is in collision");
01045             } else if (state_error_code.val == state_error_code.PATH_CONSTRAINTS_VIOLATED) {
01046               ROS_WARN("Though trajectory is done current state violates path constraints");
01047             } else if (state_error_code.val == state_error_code.JOINT_LIMITS_VIOLATED) {
01048               ROS_WARN("Though trajectory is done current state violates joint limits");
01049             } else if(state_error_code.val == state_error_code.GOAL_CONSTRAINTS_VIOLATED) {
01050               ROS_WARN("Though trajectory is done current state does not seem to be at goal");
01051             }
01052             resetStateMachine();
01053             action_server_->setAborted(move_arm_action_result_);
01054             ROS_INFO("Monitor done but not in good state");
01055             return true;              
01056           }
01057         }
01058         break;
01059       }
01060     default:
01061       {
01062         ROS_INFO("Should not be here.");
01063         break;
01064       }
01065     }   
01066     if(!action_server_->isActive())
01067     {
01068       ROS_DEBUG("Move arm no longer has an active goal");
01069       return true;
01070     }
01071     return false;               
01072   }
01073   void execute(const arm_navigation_msgs::MoveArmGoalConstPtr& goal)
01074   {
01075     arm_navigation_msgs::GetMotionPlan::Request req;        
01076     moveArmGoalToPlannerRequest(goal,req);          
01077 
01078     if(!getAndSetPlanningScene(goal->planning_scene_diff, goal->operations)) {
01079       ROS_INFO("Problem setting planning scene");
01080       move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.INCOMPLETE_ROBOT_STATE;
01081       action_server_->setAborted(move_arm_action_result_);
01082       return;
01083     }
01084 
01085     collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_,
01086                                                                 req.motion_plan_request.goal_constraints);
01087 
01088     collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_,
01089                                                                 req.motion_plan_request.path_constraints);
01090 
01091     //overwriting start state - move arm only deals with current state state
01092     planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01093                                                             ros::Time::now(),
01094                                                             collision_models_->getWorldFrameId(),
01095                                                             req.motion_plan_request.start_state);
01096     original_request_ = req;
01097 
01098     ros::Rate move_arm_rate(move_arm_frequency_);
01099     move_arm_action_result_.contacts.clear();
01100     move_arm_action_result_.error_code.val = 0;
01101     move_arm_stats_.time_to_execution = ros::Time::now().toSec();
01102     move_arm_stats_.time_to_result = ros::Time::now().toSec();
01103     while(private_handle_.ok())
01104     {               
01105       if (action_server_->isPreemptRequested())
01106       {
01107         revertPlanningScene();
01108         move_arm_stats_.preempted = true;
01109         if(publish_stats_)
01110           publishStats();
01111         move_arm_stats_.time_to_execution = ros::Time::now().toSec();
01112         move_arm_stats_.time_to_result = ros::Time::now().toSec();
01113         if(action_server_->isNewGoalAvailable())
01114         {
01115           move_arm_action_result_.contacts.clear();
01116           move_arm_action_result_.error_code.val = 0;
01117           const arm_navigation_msgs::MoveArmGoalConstPtr& new_goal = action_server_->acceptNewGoal();
01118           moveArmGoalToPlannerRequest(new_goal,req);
01119           ROS_DEBUG("Received new goal, will preempt previous goal");
01120           if(!getAndSetPlanningScene(new_goal->planning_scene_diff, new_goal->operations)) {
01121             ROS_INFO("Problem setting planning scene");
01122             move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.INCOMPLETE_ROBOT_STATE;
01123             action_server_->setAborted(move_arm_action_result_);
01124             return;
01125           }
01126           
01127           collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_,
01128                                                                       req.motion_plan_request.goal_constraints);
01129           
01130           collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_,
01131                                                                       req.motion_plan_request.path_constraints);
01132           planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01133                                                                   ros::Time::now(),
01134                                                                   collision_models_->getWorldFrameId(),
01135                                                                   req.motion_plan_request.start_state);
01136           original_request_ = req;
01137 
01138           state_ = PLANNING;
01139         }
01140         else               //if we've been preempted explicitly we need to shut things down
01141         {
01142           ROS_DEBUG("The move arm action was preempted by the action client. Preempting this goal.");
01143           revertPlanningScene();
01144           resetStateMachine();
01145           action_server_->setPreempted();
01146           return;
01147         }
01148       }
01149 
01150       //for timing that gives real time even in simulation
01151       ros::WallTime start = ros::WallTime::now();
01152 
01153       //the real work on pursuing a goal is done here
01154       bool done = executeCycle(req);
01155 
01156 
01157       if(done)
01158       {
01159         if(publish_stats_)
01160           publishStats();
01161         return;
01162       }
01163 
01164       ros::WallDuration t_diff = ros::WallTime::now() - start;
01165       ROS_DEBUG("Full control cycle time: %.9f\n", t_diff.toSec());
01166 
01167       move_arm_rate.sleep();
01168     }       
01169     //if the node is killed then we'll abort and return
01170     ROS_INFO("Node was killed, aborting");
01171     action_server_->setAborted(move_arm_action_result_);
01172   }
01173 
01174   bool getAndSetPlanningScene(const arm_navigation_msgs::PlanningScene& planning_diff,
01175                               const arm_navigation_msgs::OrderedCollisionOperations& operations) {
01176     arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
01177     arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
01178 
01179     revertPlanningScene();
01180 
01181     planning_scene_req.planning_scene_diff = planning_diff;
01182     planning_scene_req.operations = operations;
01183 
01184     if(!set_planning_scene_diff_client_.call(planning_scene_req, planning_scene_res)) {
01185       ROS_WARN("Can't get planning scene");
01186       return false;
01187     }
01188 
01189     current_planning_scene_ = planning_scene_res.planning_scene;
01190 
01191     planning_scene_state_ = collision_models_->setPlanningScene(current_planning_scene_);
01192 
01193     collision_models_->disableCollisionsForNonUpdatedLinks(group_);
01194 
01195     if(planning_scene_state_ == NULL) {
01196       ROS_WARN("Problems setting local state");
01197       return false;
01198     }    
01199     return true;
01200   }
01201 
01202   void resetToStartState(planning_models::KinematicState* state) {
01203     planning_environment::setRobotStateAndComputeTransforms(current_planning_scene_.robot_state, *state);
01204   }
01205 
01206   bool revertPlanningScene() {
01207     if(planning_scene_state_ != NULL) {
01208       collision_models_->revertPlanningScene(planning_scene_state_);
01209       planning_scene_state_ = NULL;
01210     }
01211     return true;
01212   }
01213 
01217         
01221   void publishStats()
01222   {
01223     move_arm_stats_.error_code.val = move_arm_action_result_.error_code.val;
01224     move_arm_stats_.result = arm_navigation_msgs::armNavigationErrorCodeToString(move_arm_action_result_.error_code);
01225     stats_publisher_.publish(move_arm_stats_);
01226     // Reset
01227     move_arm_stats_.error_code.val = 0;
01228     move_arm_stats_.result = " ";
01229     move_arm_stats_.request_id++;
01230     move_arm_stats_.planning_time = -1.0;
01231     move_arm_stats_.smoothing_time = -1.0;
01232     move_arm_stats_.ik_time = -1.0;
01233     move_arm_stats_.time_to_execution = -1.0;
01234     move_arm_stats_.time_to_result = -1.0;
01235     move_arm_stats_.preempted = false;
01236     move_arm_stats_.num_replans = 0.0;
01237     move_arm_stats_.trajectory_duration = -1.0;
01238   }
01239 
01240   void printTrajectory(const trajectory_msgs::JointTrajectory &trajectory)
01241   {
01242     for (unsigned int i = 0 ; i < trajectory.points.size() ; ++i)
01243     {
01244       std::stringstream ss;
01245       for (unsigned int j = 0 ; j < trajectory.points[i].positions.size() ; ++j)
01246         ss << trajectory.points[i].positions[j] << " ";
01247       ss << trajectory.points[i].time_from_start.toSec();
01248       ROS_DEBUG("%s", ss.str().c_str());
01249     }
01250   }      
01251   void visualizeJointGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
01252   {
01253     //if(!isJointGoal(req))
01254     //{
01255     //  ROS_WARN("Only joint goals can be displayed");
01256     //  return;
01257     //}
01258     ROS_DEBUG("Displaying joint goal");
01259     arm_navigation_msgs::DisplayTrajectory d_path;
01260     d_path.model_id = req.motion_plan_request.group_name;
01261     d_path.trajectory.joint_trajectory = arm_navigation_msgs::jointConstraintsToJointTrajectory(req.motion_plan_request.goal_constraints.joint_constraints);
01262     resetToStartState(planning_scene_state_);
01263     planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01264                                                             ros::Time::now(),
01265                                                             collision_models_->getWorldFrameId(),
01266                                                             d_path.robot_state);
01267     display_joint_goal_publisher_.publish(d_path);
01268     ROS_DEBUG("Displaying move arm joint goal.");
01269   }
01270   void visualizeJointGoal(const trajectory_msgs::JointTrajectory &trajectory)
01271   {
01272     ROS_DEBUG("Displaying joint goal");
01273     arm_navigation_msgs::DisplayTrajectory d_path;
01274     d_path.trajectory.joint_trajectory = trajectory;
01275     resetToStartState(planning_scene_state_);
01276     planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01277                                                             ros::Time::now(),
01278                                                             collision_models_->getWorldFrameId(),
01279                                                             d_path.robot_state);
01280     display_joint_goal_publisher_.publish(d_path);
01281   }
01282   void visualizePlan(const trajectory_msgs::JointTrajectory &trajectory)
01283   {
01284     move_arm_action_feedback_.state = "visualizing plan";
01285     if(action_server_->isActive())
01286       action_server_->publishFeedback(move_arm_action_feedback_);
01287     arm_navigation_msgs::DisplayTrajectory d_path;
01288     d_path.model_id = original_request_.motion_plan_request.group_name;
01289     d_path.trajectory.joint_trajectory = trajectory;
01290     resetToStartState(planning_scene_state_);
01291     planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01292                                                             ros::Time::now(),
01293                                                             collision_models_->getWorldFrameId(),
01294                                                             d_path.robot_state);
01295     display_path_publisher_.publish(d_path);
01296   }
01297   void visualizeAllowedContactRegions(const std::vector<arm_navigation_msgs::AllowedContactSpecification> &allowed_contacts)
01298   {
01299     static int count = 0;
01300     visualization_msgs::MarkerArray mk;
01301     mk.markers.resize(allowed_contacts.size());
01302     for(unsigned int i=0; i < allowed_contacts.size(); i++) 
01303     { 
01304       bool valid_shape = true;
01305       mk.markers[i].header.stamp = ros::Time::now();
01306       mk.markers[i].header.frame_id = allowed_contacts[i].pose_stamped.header.frame_id;
01307       mk.markers[i].ns = "move_arm::"+allowed_contacts[i].name;
01308       mk.markers[i].id = count++;
01309       if(allowed_contacts[i].shape.type == arm_navigation_msgs::Shape::SPHERE)
01310       {        
01311         mk.markers[i].type = visualization_msgs::Marker::SPHERE;
01312         if(allowed_contacts[i].shape.dimensions.size() >= 1)
01313           mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[0];
01314         else
01315           valid_shape = false;
01316       }      
01317       else if (allowed_contacts[i].shape.type == arm_navigation_msgs::Shape::BOX)
01318       {
01319         mk.markers[i].type = visualization_msgs::Marker::CUBE;
01320         if(allowed_contacts[i].shape.dimensions.size() >= 3)
01321         {
01322           mk.markers[i].scale.x = allowed_contacts[i].shape.dimensions[0];
01323           mk.markers[i].scale.y = allowed_contacts[i].shape.dimensions[1];
01324           mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[2];
01325         }
01326         else
01327           valid_shape = false;
01328       }
01329       else if (allowed_contacts[i].shape.type == arm_navigation_msgs::Shape::CYLINDER)
01330       {
01331         mk.markers[i].type = visualization_msgs::Marker::CYLINDER;
01332         if(allowed_contacts[i].shape.dimensions.size() >= 2)
01333         {
01334           mk.markers[i].scale.x = allowed_contacts[i].shape.dimensions[0];
01335           mk.markers[i].scale.y = allowed_contacts[i].shape.dimensions[0];
01336           mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[1];
01337         }
01338         else
01339           valid_shape = false;
01340       }
01341       else
01342       {
01343         mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = 0.01;
01344         valid_shape = false;
01345       }        
01346 
01347       mk.markers[i].action = visualization_msgs::Marker::ADD;
01348       mk.markers[i].pose = allowed_contacts[i].pose_stamped.pose;  
01349       if(!valid_shape)
01350       {
01351         mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = 0.01;
01352         mk.markers[i].color.a = 0.3;
01353         mk.markers[i].color.r = 1.0;
01354         mk.markers[i].color.g = 0.04;
01355         mk.markers[i].color.b = 0.04;
01356       }
01357       else
01358       {
01359         mk.markers[i].color.a = 1.0;
01360         mk.markers[i].color.r = 0.04;
01361         mk.markers[i].color.g = 1.0;
01362         mk.markers[i].color.b = 0.04;
01363       }  
01364       //mk.markers[i].lifetime = ros::Duration(30.0);  
01365     }
01366     allowed_contact_regions_publisher_.publish(mk);
01367   }
01371 
01372 private:
01373 
01374   std::string group_;
01375 
01376   ros::ServiceClient ik_client_;
01377   ros::ServiceClient trajectory_start_client_,trajectory_cancel_client_,trajectory_query_client_;       
01378   ros::NodeHandle private_handle_, root_handle_;
01379   boost::shared_ptr<actionlib::SimpleActionServer<arm_navigation_msgs::MoveArmAction> > action_server_; 
01380 
01381   planning_environment::CollisionModels* collision_models_;
01382 
01383   arm_navigation_msgs::SetPlanningSceneDiff::Request set_planning_scene_diff_req_;
01384   arm_navigation_msgs::SetPlanningSceneDiff::Response set_planning_scene_diff_res_;
01385   arm_navigation_msgs::PlanningScene current_planning_scene_;
01386   planning_models::KinematicState* planning_scene_state_;
01387 
01388   tf::TransformListener *tf_;
01389   MoveArmState state_;
01390   double move_arm_frequency_;           
01391   trajectory_msgs::JointTrajectory current_trajectory_;
01392 
01393   int num_planning_attempts_;
01394 
01395   std::vector<std::string> group_joint_names_;
01396   std::vector<std::string> group_link_names_;
01397   std::vector<std::string> all_link_names_;
01398   arm_navigation_msgs::MoveArmResult move_arm_action_result_;
01399   arm_navigation_msgs::MoveArmFeedback move_arm_action_feedback_;
01400 
01401   arm_navigation_msgs::GetMotionPlan::Request original_request_;
01402 
01403   ros::Publisher display_path_publisher_;
01404   ros::Publisher display_joint_goal_publisher_;
01405   ros::Publisher allowed_contact_regions_publisher_;
01406   ros::Publisher vis_marker_publisher_;
01407   ros::Publisher vis_marker_array_publisher_;
01408   ros::ServiceClient filter_trajectory_client_;
01409   ros::ServiceClient get_state_client_;
01410   ros::ServiceClient set_planning_scene_diff_client_;
01411   MoveArmParameters move_arm_parameters_;
01412 
01413   ControllerStatus controller_status_;
01414 
01415   JointExecutorActionClient* controller_action_client_;
01416   JointExecutorActionClient::GoalHandle controller_goal_handle_;
01417 
01418   double trajectory_filter_allowed_time_, ik_allowed_time_;
01419   double trajectory_discretization_;
01420   bool arm_ik_initialized_;
01421   
01422   bool publish_stats_;
01423   arm_navigation_msgs::MoveArmStatistics move_arm_stats_;
01424   ros::Publisher stats_publisher_;
01425   
01426 };
01427 }
01428 
01429 int main(int argc, char** argv)
01430 {
01431   ros::init(argc, argv, "move_arm");
01432   
01433   ros::AsyncSpinner spinner(1); // Use 1 thread
01434   spinner.start();
01435   ros::NodeHandle nh("~");
01436   std::string group;
01437   nh.param<std::string>("group", group, std::string());
01438   ROS_INFO("Move arm operating on group %s",group.c_str());    
01439   move_arm::MoveArm move_arm(group);
01440   if(!move_arm.configure())
01441   {
01442     ROS_ERROR("Could not configure move arm, exiting");
01443     ros::shutdown();
01444     return 1;
01445   }
01446   ROS_INFO("Move arm action started");
01447   ros::waitForShutdown();
01448     
01449   return 0;
01450 }


move_arm
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Mon Dec 2 2013 12:35:17