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/ompl_ros_state_validity_checker.h>
00038
00039 namespace ompl_ros_interface
00040 {
00041 void OmplRosStateValidityChecker::configureOnRequest(planning_models::KinematicState *kinematic_state,
00042 planning_models::KinematicState::JointStateGroup *joint_state_group,
00043 const motion_planning_msgs::GetMotionPlan::Request &request)
00044 {
00045 kinematic_state_ = kinematic_state;
00046 joint_state_group_ = joint_state_group;
00047
00048 goal_constraint_evaluator_set_.clear();
00049 path_constraint_evaluator_set_.clear();
00050
00051
00052 motion_planning_msgs::Constraints goal_constraints = getPhysicalConstraints(request.motion_plan_request.goal_constraints);
00053 motion_planning_msgs::Constraints path_constraints = getPhysicalConstraints(request.motion_plan_request.path_constraints);
00054
00055 goal_constraint_evaluator_set_.add(goal_constraints.joint_constraints);
00056 goal_constraint_evaluator_set_.add(goal_constraints.position_constraints);
00057 goal_constraint_evaluator_set_.add(goal_constraints.orientation_constraints);
00058 goal_constraint_evaluator_set_.add(goal_constraints.visibility_constraints);
00059
00060 path_constraint_evaluator_set_.add(path_constraints.joint_constraints);
00061 path_constraint_evaluator_set_.add(path_constraints.position_constraints);
00062 path_constraint_evaluator_set_.add(path_constraints.orientation_constraints);
00063 path_constraint_evaluator_set_.add(path_constraints.visibility_constraints);
00064 }
00065
00066 motion_planning_msgs::Constraints OmplRosStateValidityChecker::getPhysicalConstraints(const motion_planning_msgs::Constraints &constraints)
00067 {
00068 motion_planning_msgs::Constraints result_constraints;
00069 for(unsigned int i=0; i < constraints.joint_constraints.size(); i++)
00070 if(planning_monitor_->getKinematicModel()->hasJointModel(constraints.joint_constraints[i].joint_name))
00071 result_constraints.joint_constraints.push_back(constraints.joint_constraints[i]);
00072
00073 for(unsigned int i=0; i < constraints.position_constraints.size(); i++)
00074 if(planning_monitor_->getKinematicModel()->hasLinkModel(constraints.position_constraints[i].link_name))
00075 result_constraints.position_constraints.push_back(constraints.position_constraints[i]);
00076
00077 for(unsigned int i=0; i < constraints.orientation_constraints.size(); i++)
00078 if(planning_monitor_->getKinematicModel()->hasLinkModel(constraints.orientation_constraints[i].link_name))
00079 result_constraints.orientation_constraints.push_back(constraints.orientation_constraints[i]);
00080
00081 for(unsigned int i=0; i < constraints.visibility_constraints.size(); i++)
00082 if(planning_monitor_->getKinematicModel()->hasLinkModel(constraints.visibility_constraints[i].sensor_pose.header.frame_id))
00083 result_constraints.visibility_constraints.push_back(constraints.visibility_constraints[i]);
00084
00085 return result_constraints;
00086 }
00087
00088 void OmplRosStateValidityChecker::printSettings(std::ostream &out) const
00089 {
00090 out << "ROS State Validity Checker" << std::endl;
00091 }
00092
00093 }