$search
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 move_arm_stats_.trajectory_duration = (res.trajectory.points.back().time_from_start-res.trajectory.points.front().time_from_start).toSec(); 00431 move_arm_stats_.smoothing_time = (ros::Time::now()-smoothing_time).toSec(); 00432 trajectory_out = res.trajectory; 00433 return true; 00434 } 00435 else 00436 { 00437 ROS_ERROR("Service call to filter trajectory failed."); 00438 return false; 00439 } 00440 } 00441 00445 00446 void discretizeTrajectory(const trajectory_msgs::JointTrajectory &trajectory, 00447 trajectory_msgs::JointTrajectory &trajectory_out, 00448 const double &trajectory_discretization) 00449 { 00450 trajectory_out.joint_names = trajectory.joint_names; 00451 for(unsigned int i=1; i < trajectory.points.size(); i++) 00452 { 00453 double diff = 0.0; 00454 for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++) 00455 { 00456 double start = trajectory.points[i-1].positions[j]; 00457 double end = trajectory.points[i].positions[j]; 00458 if(fabs(end-start) > diff) 00459 diff = fabs(end-start); 00460 } 00461 int num_intervals =(int) (diff/trajectory_discretization+1.0); 00462 00463 for(unsigned int k=0; k < (unsigned int) num_intervals; k++) 00464 { 00465 trajectory_msgs::JointTrajectoryPoint point; 00466 for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++) 00467 { 00468 double start = trajectory.points[i-1].positions[j]; 00469 double end = trajectory.points[i].positions[j]; 00470 point.positions.push_back(start + (end-start)*k/num_intervals); 00471 } 00472 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); 00473 trajectory_out.points.push_back(point); 00474 } 00475 } 00476 trajectory_out.points.push_back(trajectory.points.back()); 00477 } 00481 00485 bool isPoseGoal(arm_navigation_msgs::GetMotionPlan::Request &req) 00486 { 00487 if (req.motion_plan_request.goal_constraints.joint_constraints.empty() && // we have no joint constraints on the goal, 00488 req.motion_plan_request.goal_constraints.position_constraints.size() == 1 && // we have a single position constraint on the goal 00489 req.motion_plan_request.goal_constraints.orientation_constraints.size() == 1) // that is active on all 6 DOFs 00490 return true; 00491 else 00492 return false; 00493 } 00494 bool hasPoseGoal(arm_navigation_msgs::GetMotionPlan::Request &req) 00495 { 00496 if (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 isJointGoal(arm_navigation_msgs::GetMotionPlan::Request &req) 00503 { 00504 if (req.motion_plan_request.goal_constraints.position_constraints.empty() && 00505 req.motion_plan_request.goal_constraints.orientation_constraints.empty() && 00506 !req.motion_plan_request.goal_constraints.joint_constraints.empty()) 00507 return true; 00508 else 00509 return false; 00510 } 00511 00512 //stubbing out for now 00513 bool isExecutionSafe() { 00514 return true; 00515 } 00516 00517 bool getRobotState(planning_models::KinematicState* state) 00518 { 00519 arm_navigation_msgs::GetRobotState::Request req; 00520 arm_navigation_msgs::GetRobotState::Response res; 00521 if(get_state_client_.call(req,res)) 00522 { 00523 planning_environment::setRobotStateAndComputeTransforms(res.robot_state, *state); 00524 } 00525 else 00526 { 00527 ROS_ERROR("Service call to get robot state failed on %s", 00528 get_state_client_.getService().c_str()); 00529 return false; 00530 } 00531 return true; 00532 } 00536 00540 void moveArmGoalToPlannerRequest(const arm_navigation_msgs::MoveArmGoalConstPtr& goal, 00541 arm_navigation_msgs::GetMotionPlan::Request &req) 00542 { 00543 req.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp = ros::Time::now(); 00544 req.motion_plan_request = goal->motion_plan_request; 00545 00546 move_arm_parameters_.accept_partial_plans = goal->accept_partial_plans; 00547 move_arm_parameters_.accept_invalid_goals = goal->accept_invalid_goals; 00548 move_arm_parameters_.disable_ik = goal->disable_ik; 00549 move_arm_parameters_.disable_collision_monitoring = goal->disable_collision_monitoring; 00550 move_arm_parameters_.allowed_planning_time = goal->motion_plan_request.allowed_planning_time.toSec(); 00551 move_arm_parameters_.planner_service_name = goal->planner_service_name; 00552 // visualizeAllowedContactRegions(req.motion_plan_request.allowed_contacts); 00553 // ROS_INFO("Move arm: %d allowed contact regions",(int)req.motion_plan_request.allowed_contacts.size()); 00554 // for(unsigned int i=0; i < req.motion_plan_request.allowed_contacts.size(); i++) 00555 // { 00556 // 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); 00557 // ROS_INFO("Frame id : %s",req.motion_plan_request.allowed_contacts[i].pose_stamped.header.frame_id.c_str()); 00558 // ROS_INFO("Depth : %f",req.motion_plan_request.allowed_contacts[i].penetration_depth); 00559 // ROS_INFO("Link : %s",req.motion_plan_request.allowed_contacts[i].link_names[0].c_str()); 00560 // ROS_INFO(" "); 00561 // } 00562 } 00563 bool doPrePlanningChecks(arm_navigation_msgs::GetMotionPlan::Request &req, 00564 arm_navigation_msgs::GetMotionPlan::Response &res) 00565 { 00566 arm_navigation_msgs::Constraints empty_goal_constraints; 00567 if(planning_scene_state_ == NULL) { 00568 ROS_INFO("Can't do pre-planning checks without planning state"); 00569 } 00570 resetToStartState(planning_scene_state_); 00571 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 00572 if(!collision_models_->isKinematicStateValid(*planning_scene_state_, 00573 group_joint_names_, 00574 error_code, 00575 empty_goal_constraints, 00576 original_request_.motion_plan_request.path_constraints, 00577 true)) { 00578 if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) { 00579 move_arm_action_result_.error_code.val = error_code.START_STATE_IN_COLLISION; 00580 collision_models_->getAllCollisionsForState(*planning_scene_state_, 00581 move_arm_action_result_.contacts); 00582 ROS_ERROR("Starting state is in collision, can't plan"); 00583 visualization_msgs::MarkerArray arr; 00584 std_msgs::ColorRGBA point_color_; 00585 point_color_.a = 1.0; 00586 point_color_.r = 1.0; 00587 point_color_.g = .8; 00588 point_color_.b = 0.04; 00589 00590 collision_models_->getAllCollisionPointMarkers(*planning_scene_state_, 00591 arr, 00592 point_color_, 00593 ros::Duration(0.0)); 00594 std_msgs::ColorRGBA col; 00595 col.a = .9; 00596 col.r = 1.0; 00597 col.b = 1.0; 00598 col.g = 0.0; 00599 /* 00600 collision_models_->getRobotTrimeshMarkersGivenState(*planning_scene_state_, 00601 arr, 00602 true, 00603 ros::Duration(0.0)); 00604 */ 00605 vis_marker_array_publisher_.publish(arr); 00606 } else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) { 00607 move_arm_action_result_.error_code.val = error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS; 00608 ROS_ERROR("Starting state violated path constraints, can't plan");; 00609 } else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED) { 00610 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.JOINT_LIMITS_VIOLATED;; 00611 ROS_ERROR("Start state violates joint limits, can't plan."); 00612 } 00613 if(log_to_warehouse_) { 00614 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 00615 "start_state", 00616 move_arm_action_result_.error_code); 00617 } 00618 ROS_INFO("Setting aborted because start state invalid"); 00619 action_server_->setAborted(move_arm_action_result_); 00620 return false; 00621 } 00622 // processing and checking goal 00623 if (!move_arm_parameters_.disable_ik && isPoseGoal(req)) { 00624 ROS_INFO("Planning to a pose goal"); 00625 if(!convertPoseGoalToJointGoal(req)) { 00626 ROS_INFO("Setting aborted because ik failed"); 00627 if(log_to_warehouse_) { 00628 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 00629 "ik", 00630 move_arm_action_result_.error_code); 00631 } 00632 action_server_->setAborted(move_arm_action_result_); 00633 return false; 00634 } 00635 } 00636 //if we still have pose constraints at this point it's probably a constrained combo goal 00637 if(!hasPoseGoal(req)) { 00638 arm_navigation_msgs::RobotState empty_state; 00639 empty_state.joint_state = arm_navigation_msgs::jointConstraintsToJointState(req.motion_plan_request.goal_constraints.joint_constraints); 00640 planning_environment::setRobotStateAndComputeTransforms(empty_state, *planning_scene_state_); 00641 arm_navigation_msgs::Constraints empty_constraints; 00642 if(!collision_models_->isKinematicStateValid(*planning_scene_state_, 00643 group_joint_names_, 00644 error_code, 00645 original_request_.motion_plan_request.goal_constraints, 00646 original_request_.motion_plan_request.path_constraints, 00647 true)) { 00648 if(error_code.val == error_code.JOINT_LIMITS_VIOLATED) { 00649 ROS_ERROR("Will not plan to requested joint goal since it violates joint limits constraints"); 00650 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.JOINT_LIMITS_VIOLATED; 00651 } else if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) { 00652 ROS_ERROR("Will not plan to requested joint goal since it is in collision"); 00653 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.GOAL_IN_COLLISION; 00654 collision_models_->getAllCollisionsForState(*planning_scene_state_, 00655 move_arm_action_result_.contacts); 00656 } else if(error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) { 00657 ROS_ERROR("Will not plan to requested joint goal since it violates goal constraints"); 00658 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS; 00659 } else if(error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) { 00660 ROS_ERROR("Will not plan to requested joint goal since it violates path constraints"); 00661 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS; 00662 } else { 00663 ROS_INFO_STREAM("Will not plan to request joint goal due to error code " << error_code.val); 00664 } 00665 ROS_INFO_STREAM("Setting aborted becuase joint goal is problematic"); 00666 if(log_to_warehouse_) { 00667 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 00668 "goal_state", 00669 move_arm_action_result_.error_code); 00670 } 00671 action_server_->setAborted(move_arm_action_result_); 00672 return false; 00673 } 00674 } 00675 return true; 00676 } 00677 00678 bool createPlan(arm_navigation_msgs::GetMotionPlan::Request &req, 00679 arm_navigation_msgs::GetMotionPlan::Response &res) 00680 { 00681 while(!ros::service::waitForService(move_arm_parameters_.planner_service_name, ros::Duration(1.0))) { 00682 ROS_INFO_STREAM("Waiting for requested service " << move_arm_parameters_.planner_service_name); 00683 } 00684 ros::ServiceClient planning_client = root_handle_.serviceClient<arm_navigation_msgs::GetMotionPlan>(move_arm_parameters_.planner_service_name); 00685 move_arm_stats_.planner_service_name = move_arm_parameters_.planner_service_name; 00686 ROS_DEBUG("Issuing request for motion plan"); 00687 // call the planner and decide whether to use the path 00688 if (planning_client.call(req, res)) 00689 { 00690 if (res.trajectory.joint_trajectory.points.empty()) 00691 { 00692 ROS_WARN("Motion planner was unable to plan a path to goal"); 00693 return false; 00694 } 00695 ROS_INFO("Motion planning succeeded"); 00696 return true; 00697 } 00698 else 00699 { 00700 ROS_ERROR("Motion planning service failed on %s",planning_client.getService().c_str()); 00701 return false; 00702 } 00703 } 00707 00708 bool sendTrajectory(trajectory_msgs::JointTrajectory ¤t_trajectory) 00709 { 00710 head_monitor_done_ = false; 00711 head_monitor_error_code_.val = 0; 00712 current_trajectory.header.stamp = ros::Time::now()+ros::Duration(0.2); 00713 00714 head_monitor_msgs::HeadMonitorGoal goal; 00715 goal.group_name = group_; 00716 goal.joint_trajectory = current_trajectory; 00717 goal.target_link = head_monitor_link_; 00718 goal.time_offset = ros::Duration(head_monitor_time_offset_); 00719 00720 ROS_DEBUG("Sending trajectory for monitoring with %d points and timestamp: %f",(int)goal.joint_trajectory.points.size(),goal.joint_trajectory.header.stamp.toSec()); 00721 for(unsigned int i=0; i < goal.joint_trajectory.joint_names.size(); i++) 00722 ROS_DEBUG("Joint: %d name: %s",i,goal.joint_trajectory.joint_names[i].c_str()); 00723 00724 /* for(unsigned int i = 0; i < goal.trajectory.points.size(); i++) 00725 { 00726 ROS_INFO("%f: %f %f %f %f %f %f %f %f %f %f %f %f %f %f", 00727 goal.trajectory.points[i].time_from_start.toSec(), 00728 goal.trajectory.points[i].positions[0], 00729 goal.trajectory.points[i].positions[1], 00730 goal.trajectory.points[i].positions[2], 00731 goal.trajectory.points[i].positions[3], 00732 goal.trajectory.points[i].positions[4], 00733 goal.trajectory.points[i].positions[5], 00734 goal.trajectory.points[i].positions[6], 00735 goal.trajectory.points[i].velocities[0], 00736 goal.trajectory.points[i].velocities[1], 00737 goal.trajectory.points[i].velocities[2], 00738 goal.trajectory.points[i].velocities[3], 00739 goal.trajectory.points[i].velocities[4], 00740 goal.trajectory.points[i].velocities[5], 00741 goal.trajectory.points[i].velocities[6]); 00742 }*/ 00743 head_monitor_client_->sendGoal(goal, 00744 boost::bind(&MoveArm::monitorDoneCallback, this, _1, _2), 00745 actionlib::SimpleActionClient<head_monitor_msgs::HeadMonitorAction>::SimpleActiveCallback(), 00746 boost::bind(&MoveArm::monitorFeedbackCallback, this, _1)); 00747 00748 return true; 00749 } 00750 00751 void fillTrajectoryMsg(const trajectory_msgs::JointTrajectory &trajectory_in, 00752 trajectory_msgs::JointTrajectory &trajectory_out) 00753 { 00754 trajectory_out = trajectory_in; 00755 if(trajectory_in.points.empty()) 00756 { 00757 ROS_WARN("No points in trajectory"); 00758 return; 00759 } 00760 // get the current state 00761 double d = 0.0; 00762 00763 00764 std::map<std::string, double> val_map; 00765 resetToStartState(planning_scene_state_); 00766 planning_scene_state_->getKinematicStateValues(val_map); 00767 sensor_msgs::JointState current; 00768 current.name = trajectory_out.joint_names; 00769 current.position.resize(trajectory_out.joint_names.size()); 00770 for(unsigned int i = 0; i < trajectory_out.joint_names.size(); i++) { 00771 current.position[i] = val_map[trajectory_out.joint_names[i]]; 00772 } 00773 std::map<std::string, bool> continuous; 00774 for(unsigned int j = 0; j < trajectory_in.joint_names.size(); j++) { 00775 std::string name = trajectory_in.joint_names[j]; 00776 boost::shared_ptr<const urdf::Joint> joint = collision_models_->getParsedDescription()->getJoint(name); 00777 if (joint.get() == NULL) 00778 { 00779 ROS_ERROR("Joint name %s not found in urdf model", name.c_str()); 00780 return; 00781 } 00782 if (joint->type == urdf::Joint::CONTINUOUS) { 00783 continuous[name] = true; 00784 } else { 00785 continuous[name] = false; 00786 } 00787 } 00788 for (unsigned int i = 0 ; i < current.position.size() ; ++i) 00789 { 00790 double diff; 00791 if(!continuous[trajectory_in.joint_names[i]]) { 00792 diff = fabs(trajectory_in.points[0].positions[i] - val_map[trajectory_in.joint_names[i]]); 00793 } else { 00794 diff = angles::shortest_angular_distance(trajectory_in.points[0].positions[i],val_map[trajectory_in.joint_names[i]]); 00795 } 00796 d += diff * diff; 00797 } 00798 d = sqrt(d); 00799 // decide whether we place the current state in front of the trajectory_in 00800 int include_first = (d > 0.1) ? 1 : 0; 00801 double offset = 0.0; 00802 trajectory_out.points.resize(trajectory_in.points.size() + include_first); 00803 00804 if (include_first) 00805 { 00806 ROS_INFO("Adding current state to front of trajectory"); 00807 // ROS_INFO_STREAM("Old state " << trajectory_out.points[0].positions[0] 00808 // << trajectory_out.points[0].positions[1] 00809 // << trajectory_out.points[0].positions[2] 00810 // << trajectory_out.points[0].positions[3] 00811 // << trajectory_out.points[0].positions[4] 00812 // << trajectory_out.points[0].positions[5] 00813 // << trajectory_out.points[0].positions[6]); 00814 00815 // ROS_INFO_STREAM("Current state " << current.position[0] 00816 // << current.position[1] 00817 // << current.position[2] 00818 // << current.position[3] 00819 // << current.position[4] 00820 // << current.position[5] 00821 // << current.position[6]); 00822 trajectory_out.points[0].positions = arm_navigation_msgs::jointStateToJointTrajectoryPoint(current).positions; 00823 trajectory_out.points[0].time_from_start = ros::Duration(0.0); 00824 offset = 0.3 + d; 00825 } 00826 for (unsigned int i = 0 ; i < trajectory_in.points.size() ; ++i) 00827 { 00828 trajectory_out.points[i+include_first].time_from_start = trajectory_in.points[i].time_from_start; 00829 trajectory_out.points[i+include_first].positions = trajectory_in.points[i].positions; 00830 } 00831 trajectory_out.header.stamp = ros::Time::now(); 00832 } 00836 00840 void resetStateMachine() 00841 { 00842 num_planning_attempts_ = 0; 00843 current_trajectory_.points.clear(); 00844 current_trajectory_.joint_names.clear(); 00845 state_ = PLANNING; 00846 } 00847 bool executeCycle(arm_navigation_msgs::GetMotionPlan::Request &req) 00848 { 00849 arm_navigation_msgs::GetMotionPlan::Response res; 00850 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 00851 00852 switch(state_) 00853 { 00854 case PLANNING: 00855 { 00856 move_arm_action_feedback_.state = "planning"; 00857 move_arm_action_feedback_.time_to_completion = ros::Duration(req.motion_plan_request.allowed_planning_time); 00858 action_server_->publishFeedback(move_arm_action_feedback_); 00859 00860 if(!doPrePlanningChecks(req,res)) 00861 return true; 00862 00863 visualizeJointGoal(req); 00864 resetToStartState(planning_scene_state_); 00865 if(collision_models_->isKinematicStateValid(*planning_scene_state_, 00866 group_joint_names_, 00867 error_code, 00868 original_request_.motion_plan_request.goal_constraints, 00869 original_request_.motion_plan_request.path_constraints, 00870 false)) { 00871 resetStateMachine(); 00872 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.SUCCESS; 00873 if(log_to_warehouse_) { 00874 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 00875 "start_state_at_goal", 00876 move_arm_action_result_.error_code); 00877 } 00878 action_server_->setSucceeded(move_arm_action_result_); 00879 ROS_INFO("Apparently start state satisfies goal"); 00880 return true; 00881 } 00882 ros::Time planning_time = ros::Time::now(); 00883 if(createPlan(req,res)) 00884 { 00885 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> traj_error_codes; 00886 move_arm_stats_.planning_time = (ros::Time::now()-planning_time).toSec(); 00887 ROS_DEBUG("createPlan succeeded"); 00888 resetToStartState(planning_scene_state_); 00889 if(!collision_models_->isJointTrajectoryValid(*planning_scene_state_, 00890 res.trajectory.joint_trajectory, 00891 original_request_.motion_plan_request.goal_constraints, 00892 original_request_.motion_plan_request.path_constraints, 00893 error_code, 00894 traj_error_codes, 00895 true)) 00896 { 00897 if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) { 00898 ROS_WARN("Planner trajectory collides"); 00899 } else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) { 00900 ROS_WARN("Planner trajectory violates path constraints"); 00901 } else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED) { 00902 ROS_WARN("Planner trajectory violates joint limits"); 00903 } else if (error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) { 00904 ROS_WARN("Planner trajectory doesn't reach goal"); 00905 } 00906 if(log_to_warehouse_) { 00907 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 00908 "planner invalid", 00909 error_code); 00910 } 00911 num_planning_attempts_++; 00912 if(num_planning_attempts_ > req.motion_plan_request.num_planning_attempts) 00913 { 00914 resetStateMachine(); 00915 ROS_INFO_STREAM("Setting aborted because we're out of planning attempts"); 00916 action_server_->setAborted(move_arm_action_result_); 00917 return true; 00918 } 00919 } 00920 else{ 00921 ROS_DEBUG("Trajectory validity check was successful"); 00922 00923 current_trajectory_ = res.trajectory.joint_trajectory; 00924 visualizePlan(current_trajectory_); 00925 // printTrajectory(current_trajectory_); 00926 state_ = START_CONTROL; 00927 ROS_DEBUG("Done planning. Transitioning to control"); 00928 } 00929 if(log_to_warehouse_) { 00930 warehouse_logger_->pushJointTrajectoryToWarehouse(current_planning_scene_id_, 00931 "planner", 00932 ros::Duration(move_arm_stats_.planning_time), 00933 res.trajectory.joint_trajectory, 00934 max_trajectory_ID_++, 00935 last_mpr_ID_, 00936 error_code); 00937 } 00938 } 00939 else if(action_server_->isActive()) 00940 { 00941 num_planning_attempts_++; 00942 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 00943 error_code.val = error_code.PLANNING_FAILED; 00944 if(log_to_warehouse_) { 00945 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 00946 "planner failed", 00947 error_code); 00948 } 00949 if(num_planning_attempts_ > req.motion_plan_request.num_planning_attempts) 00950 { 00951 resetStateMachine(); 00952 ROS_INFO_STREAM("Setting aborted because we're out of planning attempts"); 00953 action_server_->setAborted(move_arm_action_result_); 00954 return true; 00955 } 00956 } 00957 else 00958 { 00959 ROS_ERROR("create plan failed"); 00960 } 00961 break; 00962 } 00963 case START_CONTROL: 00964 { 00965 move_arm_action_feedback_.state = "start_control"; 00966 move_arm_action_feedback_.time_to_completion = ros::Duration(1.0/move_arm_frequency_); 00967 action_server_->publishFeedback(move_arm_action_feedback_); 00968 ROS_DEBUG("Filtering Trajectory"); 00969 trajectory_msgs::JointTrajectory filtered_trajectory; 00970 if(filterTrajectory(current_trajectory_, filtered_trajectory)) 00971 { 00972 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 00973 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> traj_error_codes; 00974 resetToStartState(planning_scene_state_); 00975 00976 if(!collision_models_->isJointTrajectoryValid(*planning_scene_state_, 00977 filtered_trajectory, 00978 original_request_.motion_plan_request.goal_constraints, 00979 original_request_.motion_plan_request.path_constraints, 00980 error_code, 00981 traj_error_codes, 00982 false)) 00983 { 00984 if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) { 00985 ROS_WARN("Filtered trajectory collides"); 00986 } else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) { 00987 ROS_WARN("Filtered trajectory violates path constraints"); 00988 } else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED) { 00989 ROS_WARN("Filtered trajectory violates joint limits"); 00990 } else if (error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) { 00991 ROS_WARN("Filtered trajectory doesn't reach goal"); 00992 } 00993 ROS_ERROR("Move arm will abort this goal. Will replan"); 00994 state_ = PLANNING; 00995 num_planning_attempts_++; 00996 if(num_planning_attempts_ > req.motion_plan_request.num_planning_attempts) 00997 { 00998 resetStateMachine(); 00999 ROS_INFO_STREAM("Setting aborted because we're out of planning attempts"); 01000 action_server_->setAborted(move_arm_action_result_); 01001 return true; 01002 } 01003 //resetStateMachine(); 01004 //action_server_->setAborted(move_arm_action_result_); 01005 break; 01006 //return true; 01007 } 01008 else{ 01009 ROS_DEBUG("Trajectory validity check was successful"); 01010 } 01011 if(log_to_warehouse_) { 01012 warehouse_logger_->pushJointTrajectoryToWarehouse(current_planning_scene_id_, 01013 "filter", 01014 ros::Duration(move_arm_stats_.smoothing_time), 01015 filtered_trajectory, 01016 max_trajectory_ID_++, 01017 last_mpr_ID_, 01018 error_code); 01019 } 01020 current_trajectory_ = filtered_trajectory; 01021 } else { 01022 if(log_to_warehouse_) { 01023 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 01024 error_code.val = error_code.PLANNING_FAILED; 01025 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 01026 "filter_rejects_planner", 01027 error_code); 01028 } 01029 resetStateMachine(); 01030 ROS_INFO_STREAM("Setting aborted because trajectory filter call failed"); 01031 action_server_->setAborted(move_arm_action_result_); 01032 return true; 01033 } 01034 ROS_DEBUG("Sending trajectory"); 01035 move_arm_stats_.time_to_execution = (ros::Time::now() - ros::Time(move_arm_stats_.time_to_execution)).toSec(); 01036 if(sendTrajectory(current_trajectory_)) 01037 { 01038 state_ = MONITOR; 01039 } 01040 else 01041 { 01042 resetStateMachine(); 01043 ROS_INFO("Setting aborted because we couldn't send the trajectory"); 01044 action_server_->setAborted(move_arm_action_result_); 01045 return true; 01046 } 01047 break; 01048 } 01049 case MONITOR: 01050 { 01051 move_arm_action_feedback_.state = "monitor"; 01052 move_arm_action_feedback_.time_to_completion = current_trajectory_.points.back().time_from_start; 01053 action_server_->publishFeedback(move_arm_action_feedback_); 01054 ROS_DEBUG("Start to monitor"); 01055 if(head_monitor_done_) 01056 { 01057 move_arm_stats_.time_to_result = (ros::Time::now()-ros::Time(move_arm_stats_.time_to_result)).toSec(); 01058 01059 arm_navigation_msgs::RobotState empty_state; 01060 arm_navigation_msgs::ArmNavigationErrorCodes state_error_code; 01061 getRobotState(planning_scene_state_); 01062 if(collision_models_->isKinematicStateValid(*planning_scene_state_, 01063 group_joint_names_, 01064 state_error_code, 01065 original_request_.motion_plan_request.goal_constraints, 01066 original_request_.motion_plan_request.path_constraints, 01067 true)) 01068 { 01069 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.SUCCESS; 01070 resetStateMachine(); 01071 action_server_->setSucceeded(move_arm_action_result_); 01072 if(head_monitor_error_code_.val == head_monitor_error_code_.TRAJECTORY_CONTROLLER_FAILED) { 01073 ROS_INFO("Monitor failed but we seem to be at goal"); 01074 if(log_to_warehouse_) { 01075 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 01076 "trajectory_failed_at_goal", 01077 move_arm_action_result_.error_code); 01078 } 01079 } else { 01080 if(log_to_warehouse_) { 01081 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 01082 "ok", 01083 move_arm_action_result_.error_code); 01084 } 01085 ROS_DEBUG("Reached goal"); 01086 } 01087 return true; 01088 } 01089 else 01090 { 01091 if(state_error_code.val == state_error_code.COLLISION_CONSTRAINTS_VIOLATED) { 01092 move_arm_action_result_.error_code.val = state_error_code.START_STATE_IN_COLLISION; 01093 collision_models_->getAllCollisionsForState(*planning_scene_state_, 01094 move_arm_action_result_.contacts); 01095 ROS_WARN("Though trajectory is done current state is in collision"); 01096 } else if (state_error_code.val == state_error_code.PATH_CONSTRAINTS_VIOLATED) { 01097 ROS_WARN("Though trajectory is done current state violates path constraints"); 01098 } else if (state_error_code.val == state_error_code.JOINT_LIMITS_VIOLATED) { 01099 ROS_WARN("Though trajectory is done current state violates joint limits"); 01100 } else if(state_error_code.val == state_error_code.GOAL_CONSTRAINTS_VIOLATED) { 01101 ROS_WARN("Though trajectory is done current state does not seem to be at goal"); 01102 } 01103 if(log_to_warehouse_) { 01104 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 01105 "trajectory_failed_not_at_goal", 01106 state_error_code); 01107 } 01108 resetStateMachine(); 01109 action_server_->setAborted(move_arm_action_result_); 01110 ROS_INFO("Monitor done but not in good state"); 01111 return true; 01112 } 01113 } 01114 break; 01115 } 01116 default: 01117 { 01118 ROS_INFO("Should not be here."); 01119 break; 01120 } 01121 } 01122 if(!action_server_->isActive()) 01123 { 01124 ROS_DEBUG("Move arm no longer has an active goal"); 01125 return true; 01126 } 01127 return false; 01128 } 01129 void execute(const arm_navigation_msgs::MoveArmGoalConstPtr& goal) 01130 { 01131 arm_navigation_msgs::GetMotionPlan::Request req; 01132 moveArmGoalToPlannerRequest(goal,req); 01133 01134 head_monitor_msgs::PreplanHeadScanGoal preplan_scan_goal; 01135 preplan_scan_goal.group_name = group_; 01136 preplan_scan_goal.motion_plan_request = goal->motion_plan_request; 01137 preplan_scan_goal.head_monitor_link = head_monitor_link_; 01138 if(preplan_scan_action_client_->sendGoalAndWait(preplan_scan_goal, ros::Duration(30.0), ros::Duration(1.0)) != actionlib::SimpleClientGoalState::SUCCEEDED) { 01139 ROS_WARN_STREAM("Preplan scan failed"); 01140 } 01141 01142 if(!getAndSetPlanningScene(goal->planning_scene_diff, goal->operations)) { 01143 ROS_INFO("Problem setting planning scene"); 01144 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.INCOMPLETE_ROBOT_STATE; 01145 action_server_->setAborted(move_arm_action_result_); 01146 return; 01147 } 01148 01149 collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_, 01150 req.motion_plan_request.goal_constraints); 01151 01152 collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_, 01153 req.motion_plan_request.path_constraints); 01154 01155 //overwriting start state - move arm only deals with current state state 01156 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_, 01157 ros::Time::now(), 01158 collision_models_->getWorldFrameId(), 01159 req.motion_plan_request.start_state); 01160 original_request_ = req; 01161 01162 if(log_to_warehouse_) { 01163 last_mpr_ID_ = max_mpr_ID_; 01164 max_mpr_ID_++; 01165 warehouse_logger_->pushMotionPlanRequestToWarehouse(current_planning_scene_id_, 01166 last_mpr_ID_, 01167 "original", 01168 req.motion_plan_request); 01169 } 01170 01171 ros::Rate move_arm_rate(move_arm_frequency_); 01172 move_arm_action_result_.contacts.clear(); 01173 move_arm_action_result_.error_code.val = 0; 01174 move_arm_stats_.time_to_execution = ros::Time::now().toSec(); 01175 move_arm_stats_.time_to_result = ros::Time::now().toSec(); 01176 while(private_handle_.ok()) 01177 { 01178 if (action_server_->isPreemptRequested()) 01179 { 01180 if(log_to_warehouse_) { 01181 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.TIMED_OUT; 01182 warehouse_logger_->pushOutcomeToWarehouse(current_planning_scene_id_, 01183 "preempted", 01184 move_arm_action_result_.error_code); 01185 } 01186 revertPlanningScene(); 01187 move_arm_stats_.preempted = true; 01188 if(publish_stats_) 01189 publishStats(); 01190 move_arm_stats_.time_to_execution = ros::Time::now().toSec(); 01191 move_arm_stats_.time_to_result = ros::Time::now().toSec(); 01192 if(action_server_->isNewGoalAvailable()) 01193 { 01194 move_arm_action_result_.contacts.clear(); 01195 move_arm_action_result_.error_code.val = 0; 01196 const arm_navigation_msgs::MoveArmGoalConstPtr& new_goal = action_server_->acceptNewGoal(); 01197 moveArmGoalToPlannerRequest(new_goal,req); 01198 ROS_DEBUG("Received new goal, will preempt previous goal"); 01199 if(!getAndSetPlanningScene(new_goal->planning_scene_diff, new_goal->operations)) { 01200 ROS_INFO("Problem setting planning scene"); 01201 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.INCOMPLETE_ROBOT_STATE; 01202 action_server_->setAborted(move_arm_action_result_); 01203 return; 01204 } 01205 01206 collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_, 01207 req.motion_plan_request.goal_constraints); 01208 01209 collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_, 01210 req.motion_plan_request.path_constraints); 01211 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_, 01212 ros::Time::now(), 01213 collision_models_->getWorldFrameId(), 01214 req.motion_plan_request.start_state); 01215 original_request_ = req; 01216 01217 if(log_to_warehouse_) { 01218 last_mpr_ID_ = max_mpr_ID_; 01219 max_mpr_ID_++; 01220 warehouse_logger_->pushMotionPlanRequestToWarehouse(current_planning_scene_id_, 01221 last_mpr_ID_, 01222 "original", 01223 req.motion_plan_request); 01224 } 01225 state_ = PLANNING; 01226 } 01227 else //if we've been preempted explicitly we need to shut things down 01228 { 01229 ROS_DEBUG("The move arm action was preempted by the action client. Preempting this goal."); 01230 if(state_ == MONITOR) { 01231 head_monitor_client_->cancelGoal(); 01232 } 01233 revertPlanningScene(); 01234 resetStateMachine(); 01235 action_server_->setPreempted();//move_arm_action_result_); 01236 return; 01237 } 01238 } 01239 01240 //for timing that gives real time even in simulation 01241 ros::WallTime start = ros::WallTime::now(); 01242 01243 //the real work on pursuing a goal is done here 01244 bool done = executeCycle(req); 01245 01246 01247 if(done) 01248 { 01249 if(publish_stats_) 01250 publishStats(); 01251 return; 01252 } 01253 01254 ros::WallDuration t_diff = ros::WallTime::now() - start; 01255 ROS_DEBUG("Full control cycle time: %.9f\n", t_diff.toSec()); 01256 01257 move_arm_rate.sleep(); 01258 } 01259 //if the node is killed then we'll abort and return 01260 ROS_INFO("Node was killed, aborting"); 01261 action_server_->setAborted(move_arm_action_result_); 01262 } 01263 01264 void monitorDoneCallback(const actionlib::SimpleClientGoalState& state, 01265 const head_monitor_msgs::HeadMonitorResultConstPtr& result) { 01266 //TODO - parse goal state for success or failure 01267 ROS_DEBUG("In monitor done callback"); 01268 head_monitor_done_ = true; 01269 if (result) { 01270 head_monitor_error_code_ = result->error_code; 01271 ROS_DEBUG_STREAM("Actual trajectory with " << result->actual_trajectory.points.size()); 01272 } else { 01273 ROS_ERROR("Result was NULL in monitor done callback!"); 01274 } 01275 if(log_to_warehouse_) { 01276 warehouse_logger_->pushJointTrajectoryToWarehouse(current_planning_scene_id_, 01277 "monitor", 01278 result->actual_trajectory.points.back().time_from_start, 01279 result->actual_trajectory, 01280 max_trajectory_ID_++, last_mpr_ID_,result->error_code); 01281 } 01282 } 01283 01284 void monitorFeedbackCallback(const head_monitor_msgs::HeadMonitorFeedbackConstPtr& feedback) { 01285 ROS_DEBUG_STREAM("Got feedback from monitor"); 01286 if(log_to_warehouse_) { 01287 warehouse_logger_->pushPausedStateToWarehouse(current_planning_scene_id_, 01288 *feedback); 01289 } 01290 } 01291 01292 bool getAndSetPlanningScene(const arm_navigation_msgs::PlanningScene& planning_diff, 01293 const arm_navigation_msgs::OrderedCollisionOperations& operations) { 01294 arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req; 01295 arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res; 01296 01297 revertPlanningScene(); 01298 01299 max_mpr_ID_ = 0; 01300 max_trajectory_ID_ = 0; 01301 01302 planning_scene_req.operations = operations; 01303 planning_scene_req.planning_scene_diff = planning_diff; 01304 01305 ROS_DEBUG_STREAM("Getting and setting planning scene"); 01306 01307 if(!set_planning_scene_diff_client_.call(planning_scene_req, planning_scene_res)) { 01308 ROS_WARN("Can't get planning scene"); 01309 return false; 01310 } 01311 01312 current_planning_scene_ = planning_scene_res.planning_scene; 01313 01314 if(log_to_warehouse_) { 01315 // Change time stamp to avoid saving sim time. 01316 current_planning_scene_.robot_state.joint_state.header.stamp = ros::Time(ros::WallTime::now().toSec()); 01317 warehouse_logger_->pushPlanningSceneToWarehouseWithoutId(current_planning_scene_, current_planning_scene_id_); 01318 } 01319 01320 planning_scene_state_ = collision_models_->setPlanningScene(current_planning_scene_); 01321 01322 collision_models_->disableCollisionsForNonUpdatedLinks(group_); 01323 01324 if(planning_scene_state_ == NULL) { 01325 ROS_WARN("Problems setting local state"); 01326 return false; 01327 } 01328 return true; 01329 } 01330 01331 void resetToStartState(planning_models::KinematicState* state) { 01332 planning_environment::setRobotStateAndComputeTransforms(current_planning_scene_.robot_state, *state); 01333 } 01334 01335 bool revertPlanningScene() { 01336 if(planning_scene_state_ != NULL) { 01337 collision_models_->revertPlanningScene(planning_scene_state_); 01338 planning_scene_state_ = NULL; 01339 } 01340 return true; 01341 } 01342 01346 01350 void publishStats() 01351 { 01352 move_arm_stats_.error_code.val = move_arm_action_result_.error_code.val; 01353 move_arm_stats_.result = arm_navigation_msgs::armNavigationErrorCodeToString(move_arm_action_result_.error_code); 01354 stats_publisher_.publish(move_arm_stats_); 01355 // Reset 01356 move_arm_stats_.error_code.val = 0; 01357 move_arm_stats_.result = " "; 01358 move_arm_stats_.request_id++; 01359 move_arm_stats_.planning_time = -1.0; 01360 move_arm_stats_.smoothing_time = -1.0; 01361 move_arm_stats_.ik_time = -1.0; 01362 move_arm_stats_.time_to_execution = -1.0; 01363 move_arm_stats_.time_to_result = -1.0; 01364 move_arm_stats_.preempted = false; 01365 move_arm_stats_.num_replans = 0.0; 01366 move_arm_stats_.trajectory_duration = -1.0; 01367 } 01368 01369 void printTrajectory(const trajectory_msgs::JointTrajectory &trajectory) 01370 { 01371 for (unsigned int i = 0 ; i < trajectory.points.size() ; ++i) 01372 { 01373 std::stringstream ss; 01374 for (unsigned int j = 0 ; j < trajectory.points[i].positions.size() ; ++j) 01375 ss << trajectory.points[i].positions[j] << " "; 01376 ss << trajectory.points[i].time_from_start.toSec(); 01377 ROS_DEBUG("%s", ss.str().c_str()); 01378 } 01379 } 01380 void visualizeJointGoal(arm_navigation_msgs::GetMotionPlan::Request &req) 01381 { 01382 //if(!isJointGoal(req)) 01383 //{ 01384 // ROS_WARN("Only joint goals can be displayed"); 01385 // return; 01386 //} 01387 ROS_DEBUG("Displaying joint goal"); 01388 arm_navigation_msgs::DisplayTrajectory d_path; 01389 d_path.model_id = req.motion_plan_request.group_name; 01390 d_path.trajectory.joint_trajectory = arm_navigation_msgs::jointConstraintsToJointTrajectory(req.motion_plan_request.goal_constraints.joint_constraints); 01391 resetToStartState(planning_scene_state_); 01392 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_, 01393 ros::Time::now(), 01394 collision_models_->getWorldFrameId(), 01395 d_path.robot_state); 01396 display_joint_goal_publisher_.publish(d_path); 01397 ROS_DEBUG("Displaying move arm joint goal."); 01398 } 01399 void visualizeJointGoal(const trajectory_msgs::JointTrajectory &trajectory) 01400 { 01401 ROS_DEBUG("Displaying joint goal"); 01402 arm_navigation_msgs::DisplayTrajectory d_path; 01403 d_path.trajectory.joint_trajectory = trajectory; 01404 resetToStartState(planning_scene_state_); 01405 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_, 01406 ros::Time::now(), 01407 collision_models_->getWorldFrameId(), 01408 d_path.robot_state); 01409 display_joint_goal_publisher_.publish(d_path); 01410 } 01411 void visualizePlan(const trajectory_msgs::JointTrajectory &trajectory) 01412 { 01413 move_arm_action_feedback_.state = "visualizing plan"; 01414 if(action_server_->isActive()) 01415 action_server_->publishFeedback(move_arm_action_feedback_); 01416 arm_navigation_msgs::DisplayTrajectory d_path; 01417 d_path.model_id = original_request_.motion_plan_request.group_name; 01418 d_path.trajectory.joint_trajectory = trajectory; 01419 resetToStartState(planning_scene_state_); 01420 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_, 01421 ros::Time::now(), 01422 collision_models_->getWorldFrameId(), 01423 d_path.robot_state); 01424 display_path_publisher_.publish(d_path); 01425 } 01426 void visualizeAllowedContactRegions(const std::vector<arm_navigation_msgs::AllowedContactSpecification> &allowed_contacts) 01427 { 01428 static int count = 0; 01429 visualization_msgs::MarkerArray mk; 01430 mk.markers.resize(allowed_contacts.size()); 01431 for(unsigned int i=0; i < allowed_contacts.size(); i++) 01432 { 01433 bool valid_shape = true; 01434 mk.markers[i].header.stamp = ros::Time::now(); 01435 mk.markers[i].header.frame_id = allowed_contacts[i].pose_stamped.header.frame_id; 01436 mk.markers[i].ns = "move_arm::"+allowed_contacts[i].name; 01437 mk.markers[i].id = count++; 01438 if(allowed_contacts[i].shape.type == arm_navigation_msgs::Shape::SPHERE) 01439 { 01440 mk.markers[i].type = visualization_msgs::Marker::SPHERE; 01441 if(allowed_contacts[i].shape.dimensions.size() >= 1) 01442 mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[0]; 01443 else 01444 valid_shape = false; 01445 } 01446 else if (allowed_contacts[i].shape.type == arm_navigation_msgs::Shape::BOX) 01447 { 01448 mk.markers[i].type = visualization_msgs::Marker::CUBE; 01449 if(allowed_contacts[i].shape.dimensions.size() >= 3) 01450 { 01451 mk.markers[i].scale.x = allowed_contacts[i].shape.dimensions[0]; 01452 mk.markers[i].scale.y = allowed_contacts[i].shape.dimensions[1]; 01453 mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[2]; 01454 } 01455 else 01456 valid_shape = false; 01457 } 01458 else if (allowed_contacts[i].shape.type == arm_navigation_msgs::Shape::CYLINDER) 01459 { 01460 mk.markers[i].type = visualization_msgs::Marker::CYLINDER; 01461 if(allowed_contacts[i].shape.dimensions.size() >= 2) 01462 { 01463 mk.markers[i].scale.x = allowed_contacts[i].shape.dimensions[0]; 01464 mk.markers[i].scale.y = allowed_contacts[i].shape.dimensions[0]; 01465 mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[1]; 01466 } 01467 else 01468 valid_shape = false; 01469 } 01470 else 01471 { 01472 mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = 0.01; 01473 valid_shape = false; 01474 } 01475 01476 mk.markers[i].action = visualization_msgs::Marker::ADD; 01477 mk.markers[i].pose = allowed_contacts[i].pose_stamped.pose; 01478 if(!valid_shape) 01479 { 01480 mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = 0.01; 01481 mk.markers[i].color.a = 0.3; 01482 mk.markers[i].color.r = 1.0; 01483 mk.markers[i].color.g = 0.04; 01484 mk.markers[i].color.b = 0.04; 01485 } 01486 else 01487 { 01488 mk.markers[i].color.a = 1.0; 01489 mk.markers[i].color.r = 0.04; 01490 mk.markers[i].color.g = 1.0; 01491 mk.markers[i].color.b = 0.04; 01492 } 01493 //mk.markers[i].lifetime = ros::Duration(30.0); 01494 } 01495 allowed_contact_regions_publisher_.publish(mk); 01496 } 01500 01501 private: 01502 01503 std::string group_; 01504 01505 boost::shared_ptr<actionlib::SimpleActionClient<head_monitor_msgs::HeadMonitorAction> > head_monitor_client_; 01506 boost::shared_ptr<actionlib::SimpleActionClient<head_monitor_msgs::PreplanHeadScanAction> > preplan_scan_action_client_; 01507 01508 ros::ServiceClient ik_client_; 01509 ros::ServiceClient trajectory_start_client_,trajectory_cancel_client_,trajectory_query_client_; 01510 ros::NodeHandle private_handle_, root_handle_; 01511 boost::shared_ptr<actionlib::SimpleActionServer<arm_navigation_msgs::MoveArmAction> > action_server_; 01512 01513 planning_environment::CollisionModels* collision_models_; 01514 01515 arm_navigation_msgs::SetPlanningSceneDiff::Request set_planning_scene_diff_req_; 01516 arm_navigation_msgs::SetPlanningSceneDiff::Response set_planning_scene_diff_res_; 01517 arm_navigation_msgs::PlanningScene current_planning_scene_; 01518 unsigned int current_planning_scene_id_; 01519 unsigned int max_mpr_ID_; 01520 unsigned int max_trajectory_ID_; 01521 unsigned int last_mpr_ID_; 01522 01523 planning_models::KinematicState* planning_scene_state_; 01524 01525 tf::TransformListener *tf_; 01526 MoveArmState state_; 01527 double move_arm_frequency_; 01528 trajectory_msgs::JointTrajectory current_trajectory_; 01529 01530 int num_planning_attempts_; 01531 01532 std::vector<std::string> group_joint_names_; 01533 std::vector<std::string> group_link_names_; 01534 std::vector<std::string> all_link_names_; 01535 arm_navigation_msgs::MoveArmResult move_arm_action_result_; 01536 arm_navigation_msgs::MoveArmFeedback move_arm_action_feedback_; 01537 01538 arm_navigation_msgs::GetMotionPlan::Request original_request_; 01539 01540 ros::Publisher display_path_publisher_; 01541 ros::Publisher display_joint_goal_publisher_; 01542 ros::Publisher allowed_contact_regions_publisher_; 01543 ros::Publisher vis_marker_publisher_; 01544 ros::Publisher vis_marker_array_publisher_; 01545 ros::ServiceClient filter_trajectory_client_; 01546 ros::ServiceClient fk_client_; 01547 ros::ServiceClient get_state_client_; 01548 ros::ServiceClient set_planning_scene_diff_client_; 01549 ros::ServiceClient log_planning_scene_client_; 01550 MoveArmParameters move_arm_parameters_; 01551 01552 double trajectory_filter_allowed_time_, ik_allowed_time_; 01553 double trajectory_discretization_; 01554 bool arm_ik_initialized_; 01555 01556 std::string head_monitor_link_; 01557 double head_monitor_time_offset_; 01558 arm_navigation_msgs::ArmNavigationErrorCodes head_monitor_error_code_; 01559 bool head_monitor_done_; 01560 01561 bool publish_stats_; 01562 arm_navigation_msgs::MoveArmStatistics move_arm_stats_; 01563 ros::Publisher stats_publisher_; 01564 01565 bool log_to_warehouse_; 01566 MoveArmWarehouseLoggerReader* warehouse_logger_; 01567 01568 }; 01569 } 01570 01571 int main(int argc, char** argv) 01572 { 01573 ros::init(argc, argv, "move_arm"); 01574 01575 ros::AsyncSpinner spinner(1); // Use 1 thread 01576 spinner.start(); 01577 ros::NodeHandle nh("~"); 01578 std::string group; 01579 nh.param<std::string>("group", group, std::string()); 01580 ROS_INFO("Move arm operating on group %s",group.c_str()); 01581 move_arm_warehouse::MoveArm move_arm(group); 01582 if(!move_arm.configure()) 01583 { 01584 ROS_ERROR("Could not configure move arm, exiting"); 01585 ros::shutdown(); 01586 return 1; 01587 } 01588 ROS_INFO("Move arm action started"); 01589 ros::waitForShutdown(); 01590 01591 return 0; 01592 }