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


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Fri Dec 6 2013 21:12:33