00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include <ompl_ros_interface/planners/ompl_ros_task_space_planner.h>
00038 #include <planning_environment/models/model_utils.h>
00039
00040 namespace ompl_ros_interface
00041 {
00042 bool OmplRosTaskSpacePlanner::initializePlanningStateSpace(ompl::base::StateSpacePtr &state_space)
00043 {
00044 if(!node_handle_.hasParam(group_name_))
00045 {
00046 ROS_ERROR("Could not find description of planning state space %s",group_name_.c_str());
00047 return false;
00048 }
00049 if(!node_handle_.getParam(group_name_+"/parent_frame", planning_frame_id_))
00050 {
00051 ROS_ERROR("Could not find parent frame for group %ss",group_name_.c_str());
00052 return false;
00053 }
00054
00055 XmlRpc::XmlRpcValue space_list;
00056 std::vector<std::string> space_names;
00057 if(!node_handle_.getParam(group_name_+"/state_spaces", space_list))
00058 {
00059 ROS_ERROR("Could not find parameter %s on param server",(group_name_+"/state_spaces").c_str());
00060 return false;
00061 }
00062 if(space_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00063 {
00064 ROS_ERROR("State space list should be of XmlRpc Array type");
00065 return false;
00066 }
00067
00068 int real_vector_index = -1;
00069 state_space.reset(new ompl::base::CompoundStateSpace());
00070 state_space->setName(group_name_);
00071 for (unsigned int i = 0; i < (unsigned int) space_list.size(); ++i)
00072 {
00073 if(space_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00074 {
00075 ROS_ERROR("State space names should be strings");
00076 return false;
00077 }
00078 space_names.push_back(static_cast<std::string>(space_list[i]));
00079 ROS_INFO("Adding state space: %s",space_names.back().c_str());
00080
00081 if(collision_models_interface_->getKinematicModel()->hasJointModel(space_names.back()))
00082 {
00083 addToOmplStateSpace(collision_models_interface_->getKinematicModel(),
00084 space_names.back(),
00085 state_space);
00086 continue;
00087 }
00088
00089 if(!getSpaceFromParamServer(ros::NodeHandle(node_handle_,group_name_+"/"+space_names.back()),
00090 space_names.back(),
00091 state_space,
00092 real_vector_index))
00093 return false;
00094 }
00095 if(!state_space)
00096 {
00097 ROS_ERROR("Could not set up the ompl state space from group %s",group_name_.c_str());
00098 return false;
00099 }
00100 return true;
00101 }
00102
00103 bool OmplRosTaskSpacePlanner::getSpaceFromParamServer(const ros::NodeHandle &node_handle,
00104 const std::string &space_name,
00105 ompl::base::StateSpacePtr& state_space,
00106 int& real_vector_index)
00107 {
00108 std::string type;
00109 if(!node_handle.hasParam("type"))
00110 {
00111 ROS_ERROR("Could not find type for state space %s",space_name.c_str());
00112 return false;
00113 }
00114 node_handle.getParam("type",type);
00115 if(type == "Revolute" || type == "Linear")
00116 {
00117 if(real_vector_index < 0)
00118 {
00119 real_vector_index = state_space->as<ompl::base::CompoundStateSpace>()->getSubspaceCount();
00120 ompl::base::RealVectorStateSpace *subspace = new ompl::base::RealVectorStateSpace(0);
00121 subspace->setName("real_vector");
00122 state_space->as<ompl::base::CompoundStateSpace>()->addSubspace(ompl::base::StateSpacePtr(subspace),1.0);
00123 }
00124 ompl::base::StateSpacePtr real_vector_state_space = state_space->as<ompl::base::CompoundStateSpace>()->getSubspace("real_vector");
00125 double min_value, max_value;
00126 node_handle.param("min",min_value,-M_PI);
00127 node_handle.param("max",max_value,M_PI);
00128 real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->addDimension(space_name,min_value,max_value);
00129 }
00130 else if(type == "Planar")
00131 {
00132 ompl::base::SE2StateSpace *subspace = new ompl::base::SE2StateSpace();
00133 subspace->setName(space_name);
00134 if(!node_handle.hasParam("x/min") || !node_handle.hasParam("x/max") ||
00135 !node_handle.hasParam("y/min") || !node_handle.hasParam("y/max"))
00136 {
00137 ROS_ERROR("Could not find bounds for planar state space %s",space_name.c_str());
00138 }
00139 ompl::base::RealVectorBounds real_vector_bounds(2);
00140 node_handle.getParam("x/min",real_vector_bounds.low[0]);
00141 node_handle.getParam("x/max",real_vector_bounds.high[0]);
00142 node_handle.getParam("y/min",real_vector_bounds.low[1]);
00143 node_handle.getParam("y/max",real_vector_bounds.high[1]);
00144 subspace->setBounds(real_vector_bounds);
00145
00146 state_space->as<ompl::base::CompoundStateSpace>()->addSubspace(ompl::base::StateSpacePtr(subspace), 1.0);
00147 }
00148 else if(type == "Floating")
00149 {
00150 ompl::base::SE3StateSpace *subspace = new ompl::base::SE3StateSpace();
00151 subspace->setName(space_name);
00152 if(!node_handle.hasParam("x/min") || !node_handle.hasParam("x/max") ||
00153 !node_handle.hasParam("y/min") || !node_handle.hasParam("y/max") ||
00154 !node_handle.hasParam("z/min") || !node_handle.hasParam("z/max"))
00155 {
00156 ROS_ERROR("Could not find bounds for floating state space %s",space_name.c_str());
00157 }
00158 ompl::base::RealVectorBounds real_vector_bounds(3);
00159 node_handle.getParam("x/min",real_vector_bounds.low[0]);
00160 node_handle.getParam("x/max",real_vector_bounds.high[0]);
00161
00162 node_handle.getParam("y/min",real_vector_bounds.low[1]);
00163 node_handle.getParam("y/max",real_vector_bounds.high[1]);
00164
00165 node_handle.getParam("z/min",real_vector_bounds.low[2]);
00166 node_handle.getParam("z/max",real_vector_bounds.high[2]);
00167
00168 subspace->setBounds(real_vector_bounds);
00169 state_space->as<ompl::base::CompoundStateSpace>()->addSubspace(ompl::base::StateSpacePtr(subspace), 1.0);
00170 }
00171 else if(type == "Continuous")
00172 {
00173 ompl::base::SO2StateSpace *subspace = new ompl::base::SO2StateSpace();
00174 subspace->setName(space_name);
00175 state_space->as<ompl::base::CompoundStateSpace>()->addSubspace(ompl::base::StateSpacePtr(subspace), 1.0);
00176 }
00177 else
00178 {
00179 ROS_ERROR("Unknown joint type: %s",type.c_str());
00180 return false;
00181 }
00182 return true;
00183 }
00184
00185 bool OmplRosTaskSpacePlanner::isRequestValid(arm_navigation_msgs::GetMotionPlan::Request &request,
00186 arm_navigation_msgs::GetMotionPlan::Response &response)
00187 {
00188 if(request.motion_plan_request.group_name != group_name_)
00189 {
00190 ROS_ERROR("Invalid group name: %s",request.motion_plan_request.group_name.c_str());
00191 response.error_code.val = response.error_code.INVALID_GROUP_NAME;
00192 return false;
00193 }
00194
00195 if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00196 request.motion_plan_request.goal_constraints))
00197 {
00198 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00199 return false;
00200 }
00201 if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00202 request.motion_plan_request.path_constraints))
00203 {
00204 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00205 return false;
00206 }
00207 if(request.motion_plan_request.allowed_planning_time.toSec() <= 0.0)
00208 {
00209 response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_TIMEOUT;
00210 ROS_ERROR("Request does not specify correct allowed planning time %f",request.motion_plan_request.allowed_planning_time.toSec());
00211 return false;
00212 }
00213 return true;
00214 }
00215
00216 bool OmplRosTaskSpacePlanner::setGoal(arm_navigation_msgs::GetMotionPlan::Request &request,
00217 arm_navigation_msgs::GetMotionPlan::Response &response)
00218 {
00219 ompl::base::ScopedState<ompl::base::CompoundStateSpace> goal(state_space_);
00220 ompl::base::GoalPtr goal_states(new ompl::base::GoalStates(planner_->getSpaceInformation()));
00221 ROS_DEBUG("Setting my goal");
00222 if(!constraintsToOmplState(request.motion_plan_request.goal_constraints,goal))
00223 {
00224 response.error_code.val = response.error_code.PLANNING_FAILED;
00225 ROS_WARN("Problem converting constraints to ompl state");
00226 return false;
00227 }
00228 goal_states->as<ompl::base::GoalStates>()->addState(goal.get());
00229 ompl_ros_interface::OmplRosTaskSpaceValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosTaskSpaceValidityChecker*>(state_validity_checker_.get());
00230 if(!my_checker->isStateValid(goal.get()))
00231 {
00232 response.error_code = my_checker->getLastErrorCode();
00233 if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED)
00234 response.error_code.val = response.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS;
00235 else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED)
00236 response.error_code.val = response.error_code.GOAL_IN_COLLISION;
00237 ROS_ERROR("Goal state is invalid. Reason: %s",arm_navigation_msgs::armNavigationErrorCodeToString(response.error_code).c_str());
00238 return false;
00239 }
00240 planner_->setGoal(goal_states);
00241 ROS_DEBUG("Setting goal state successful");
00242 return true;
00243 }
00244
00245 bool OmplRosTaskSpacePlanner::constraintsToOmplState(const arm_navigation_msgs::Constraints &constraints,
00246 ompl::base::ScopedState<ompl::base::CompoundStateSpace> &goal)
00247 {
00248 return ompl_ros_interface::constraintsToOmplState(constraints,goal);
00249 }
00250
00251 bool OmplRosTaskSpacePlanner::setStart(arm_navigation_msgs::GetMotionPlan::Request &request,
00252 arm_navigation_msgs::GetMotionPlan::Response &response)
00253 {
00254 ompl::base::ScopedState<ompl::base::CompoundStateSpace> start(state_space_);
00255 arm_navigation_msgs::RobotState cur_state;
00256 planning_environment::convertKinematicStateToRobotState(*collision_models_interface_->getPlanningSceneState(),
00257 ros::Time::now(),
00258 collision_models_interface_->getWorldFrameId(),
00259 cur_state);
00260
00261 ompl_ros_interface::robotStateToOmplState(cur_state,start,false);
00262
00263 ompl_ros_interface::OmplRosTaskSpaceValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosTaskSpaceValidityChecker*>(state_validity_checker_.get());
00264 if(!my_checker->isStateValid(start.get()))
00265 {
00266 response.error_code = my_checker->getLastErrorCode();
00267 if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED)
00268 response.error_code.val = response.error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS;
00269 else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED)
00270 response.error_code.val = response.error_code.START_STATE_IN_COLLISION;
00271 return false;
00272 }
00273 planner_->getProblemDefinition()->clearStartStates();
00274 planner_->addStartState(start);
00275 return true;
00276 }
00277
00278 bool OmplRosTaskSpacePlanner::initializeStateValidityChecker(ompl_ros_interface::OmplRosStateValidityCheckerPtr &state_validity_checker)
00279 {
00280 state_validity_checker.reset(new ompl_ros_interface::OmplRosTaskSpaceValidityChecker(planner_->getSpaceInformation().get(),
00281 collision_models_interface_,
00282 planning_frame_id_));
00283 return true;
00284 }
00285
00286
00287 }