本文整理汇总了C++中planning_scene::PlanningSceneConstPtr::getCollisionWorld方法的典型用法代码示例。如果您正苦于以下问题：C++ PlanningSceneConstPtr::getCollisionWorld方法的具体用法？C++ PlanningSceneConstPtr::getCollisionWorld怎么用？C++ PlanningSceneConstPtr::getCollisionWorld使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类planning_scene::PlanningSceneConstPtr的用法示例。
在下文中一共展示了PlanningSceneConstPtr::getCollisionWorld方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: solve
bool StompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::MotionPlanRequest &req,
moveit_msgs::MotionPlanDetailedResponse &res) const
{
ros::WallTime start_time = ros::WallTime::now();
boost::shared_ptr<StompOptimizationTask> stomp_task;
boost::shared_ptr<stomp::STOMP> stomp;
int max_rollouts;
STOMP_VERIFY(node_handle_.getParam("max_rollouts", max_rollouts));
// prepare the collision checkers
boost::shared_ptr<const collision_detection::CollisionRobot> collision_robot; /**< standard robot collision checker */
boost::shared_ptr<const collision_detection::CollisionWorld> collision_world; /**< standard robot -> world collision checker */
boost::shared_ptr<collision_detection::CollisionRobotDistanceField> collision_robot_df; /**< distance field robot collision checker */
boost::shared_ptr<collision_detection::CollisionWorldDistanceField> collision_world_df; /**< distance field robot -> world collision checker */
collision_robot = planning_scene->getCollisionRobot();
collision_world = planning_scene->getCollisionWorld();
std::map<std::string, std::vector<collision_detection::CollisionSphere> > link_body_decompositions;
bool use_signed_distance_field = true;
double padding = 0.0;
double scale = 1.0;
// TODO: remove this disgusting hack!
link_body_decompositions["r_shoulder_pan_link"] = std::vector<collision_detection::CollisionSphere>();
link_body_decompositions["r_shoulder_lift_link"] = std::vector<collision_detection::CollisionSphere>();
collision_robot_df.reset(new collision_detection::CollisionRobotDistanceField(kinematic_model_,
link_body_decompositions,
df_size_x_, df_size_y_, df_size_z_,
use_signed_distance_field,
df_resolution_, df_collision_tolerance_,
df_max_propagation_distance_,
padding, scale));
collision_world_df.reset(new collision_detection::CollisionWorldDistanceField(df_size_x_, df_size_y_, df_size_z_,
use_signed_distance_field,
df_resolution_, df_collision_tolerance_,
df_max_propagation_distance_));
copyObjects(collision_world, collision_world_df);
// first setup the task
stomp_task.reset(new StompOptimizationTask(node_handle_, req.group_name,
kinematic_model_,
collision_robot, collision_world,
collision_robot_df, collision_world_df));
int num_threads=1;
STOMP_VERIFY(stomp_task->initialize(num_threads, max_rollouts));
XmlRpc::XmlRpcValue features_xml;
STOMP_VERIFY(node_handle_.getParam("features", features_xml));
stomp_task->setFeaturesFromXml(features_xml);
stomp_task->setControlCostWeight(0.00001);
stomp_task->setTrajectoryVizPublisher(const_cast<ros::Publisher&>(trajectory_viz_pub_));
stomp_task->setDistanceFieldVizPublisher((const_cast<ros::Publisher&>(trajectory_viz_pub_)));
stomp_task->setRobotBodyVizPublisher((const_cast<ros::Publisher&>(robot_body_viz_pub_)));
stomp_task->setMotionPlanRequest(planning_scene, req);
stomp.reset(new stomp::STOMP());
stomp->initialize(node_handle_, stomp_task);
// TODO: don't hardcode these params
bool success = stomp->runUntilValid(100, 10);
std::vector<Eigen::VectorXd> best_params;
double best_cost;
stomp->getBestNoiselessParameters(best_params, best_cost);
stomp_task->publishTrajectoryMarkers(const_cast<ros::Publisher&>(trajectory_viz_pub_), best_params);
res.trajectory_start = req.start_state;
res.group_name = req.group_name;
res.trajectory.resize(1);
stomp_task->parametersToJointTrajectory(best_params, res.trajectory[0].joint_trajectory);
res.description.resize(1);
res.description[0] = "STOMP";
res.processing_time.resize(1);
ros::WallDuration wd = ros::WallTime::now() - start_time;
res.processing_time[0] = ros::Duration(wd.sec, wd.nsec);
if (!success)
{
ROS_ERROR("STOMP: failed to find a collision-free plan");
res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return true;
}
res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
// res.trajectory
// res.description
// res.processing_time
// res.error_code
return true;
}