octomap中3drrt路径规划

在octomap中制定起止点，目标点，使用rrt规划一条路径出来，没有运动学，动力学的限制，只要能避开障碍物。

效果如下(绿线是规划的路线，红线是B样条优化的曲线)：

#include ros/ros.h
#include octomap_msgs/Octomap.h
#include octomap_msgs/conversions.h
#include octomap_ros/conversions.h
#include octomap/octomap.h
#include message_filters/subscriber.h
#include visualization_msgs/Marker.h
#include trajectory_msgs/MultiDOFJointTrajectory.h
#include nav_msgs/Odometry.h
#include geometry_msgs/Pose.h
#include nav_msgs/Path.h
#include geometry_msgs/PoseStamped.h
#include ompl/base/spaces/SE3StateSpace.h
#include ompl/base/spaces/SE3StateSpace.h
#include ompl/base/OptimizationObjective.h
#include ompl/base/objectives/PathLengthOptimizationObjective.h
// #include ompl/geometric/planners/rrt/RRTstar.h
#include ompl/geometric/planners/rrt/InformedRRTstar.h
#include ompl/geometric/SimpleSetup.h
#include ompl/config.h
#include iostream
#include fcl/config.h
#include fcl/octree.h
#include fcl/traversal/traversal_node_octree.h
#include fcl/collision.h
#include fcl/broadphase/broadphase.h
#include fcl/math/transform.h
namespace ob = ompl::base;
namespace og = ompl::geometric;
// Declear some global variables
//ROS publishers
ros::Publisher vis_pub;
ros::Publisher traj_pub;
class planner {
public:
void setStart(double x, double y, double z)
{
ob::ScopedState ob::SE3StateSpace start(space);
start- setXYZ(x,y,z);
start- as ob::SO3StateSpace::StateType (1)- setIdentity();
pdef- clearStartStates();
pdef- addStartState(start);
}
void setGoal(double x, double y, double z)
{
ob::ScopedState ob::SE3StateSpace goal(space);
goal- setXYZ(x,y,z);
goal- as ob::SO3StateSpace::StateType (1)- setIdentity();
pdef- clearGoal();
pdef- setGoalState(goal);
std::cout goal set to: x y z std::endl;
}
void updateMap(std::shared_ptr fcl::CollisionGeometry map)
{
tree_obj = map;
}
// Constructor
planner(void)
{
//四旋翼的障碍物几何形状
Quadcopter = std::shared_ptr fcl::CollisionGeometry (new fcl::Box(0.8, 0.8, 0.3));
//分辨率参数设置
fcl::OcTree* tree = new fcl::OcTree(std::shared_ptr const octomap::OcTree (new octomap::OcTree(0.15)));
tree_obj = std::shared_ptr fcl::CollisionGeometry (tree);
//解的状态空间
space = ob::StateSpacePtr(new ob::SE3StateSpace());
// create a start state
ob::ScopedState ob::SE3StateSpace start(space);
// create a goal state
ob::ScopedState ob::SE3StateSpace goal(space);
// set the bounds for the R^3 part of SE(3)
// 搜索的三维范围设置
ob::RealVectorBounds bounds(3);
bounds.setLow(0,-5);
bounds.setHigh(0,5);
bounds.setLow(1,-5);
bounds.setHigh(1,5);
bounds.setLow(2,0);
bounds.setHigh(2,3);
space- as ob::SE3StateSpace ()- setBounds(bounds);
// construct an instance of space information from this state space
si = ob::SpaceInformationPtr(new ob::SpaceInformation(space));
start- setXYZ(0,0,0);
start- as ob::SO3StateSpace::StateType (1)- setIdentity();
// start.random();
goal- setXYZ(0,0,0);
goal- as ob::SO3StateSpace::StateType (1)- setIdentity();
// goal.random();
// set state validity checking for this space
si- setStateValidityChecker(std::bind( planner::isStateValid, this, std::placeholders::_1 ));
// create a problem instance
pdef = ob::ProblemDefinitionPtr(new ob::ProblemDefinition(si));
// set the start and goal states
pdef- setStartAndGoalStates(start, goal);
// set Optimizattion objective
pdef- setOptimizationObjective(planner::getPathLengthObjWithCostToGo(si));
std::cout Initialized: std::endl;
}
// Destructor
~planner()
{
}
void replan(void)
std::cout Total Points: path_smooth- getStateCount () std::endl;
if(path_smooth- getStateCount () = 2)
plan();
else
{
for (std::size_t idx = 0; idx path_smooth- getStateCount (); idx++)
{
if(!replan_flag)
replan_flag = !isStateValid(path_smooth- getState(idx));
else
break;
}
if(replan_flag)
plan();
else
std::cout Replanning not required std::endl;
}
void plan(void)
// create a planner for the defined space
og::InformedRRTstar* rrt = new og::InformedRRTstar(si);
//设置rrt的参数range
rrt- setRange(0.2);
ob::PlannerPtr plan(rrt);
// set the problem we are trying to solve for the planner
plan- setProblemDefinition(pdef);
// perform setup steps for the planner
plan- setup();
// print the settings for this space
si- printSettings(std::cout);
std::cout problem setting\n
// print the problem settings
pdef- print(std::cout);
// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = plan- solve(1);
if (solved)
{
// get the goal representation from the problem definition (not the same as the goal state)
// and inquire about the found path
std::cout Found solution: std::endl;
ob::PathPtr path = pdef- getSolutionPath();
og::PathGeometric* pth = pdef- getSolutionPath()- as og::PathGeometric
pth- printAsMatrix(std::cout);
// print the path to screen
// path- print(std::cout);
nav_msgs::Path msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = map
for (std::size_t path_idx = 0; path_idx pth- getStateCount (); path_idx++)
{
const ob::SE3StateSpace::StateType *se3state = pth- getState(path_idx)- as ob::SE3StateSpace::StateType
// extract the first component of the state and cast it to what we expect
const ob::RealVectorStateSpace::StateType *pos = se3state- as ob::RealVectorStateSpace::StateType
// extract the second component of the state and cast it to what we expect
const ob::SO3StateSpace::StateType *rot = se3state- as ob::SO3StateSpace::StateType
geometry_msgs::PoseStamped pose;
// pose.header.frame_id = /world
pose.pose.position.x = pos- values[0];
pose.pose.position.y = pos- values[1];
pose.pose.position.z = pos- values[2];
pose.pose.orientation.x = rot-
pose.pose.orientation.y = rot-
pose.pose.orientation.z = rot-
pose.pose.orientation.w = rot-
msg.poses.push_back(pose);
}
traj_pub.publish(msg);
//Path smoothing using bspline
//B样条曲线优化
og::PathSimplifier* pathBSpline = new og::PathSimplifier(si);
path_smooth = new og::PathGeometric(dynamic_cast const og::PathGeometric (*pdef- getSolutionPath()));
pathBSpline- smoothBSpline(*path_smooth,3);
// std::cout Smoothed Path std::endl;
// path_smooth.print(std::cout);
//Publish path as markers
nav_msgs::Path smooth_msg;
smooth_msg.header.stamp = ros::Time::now();
smooth_msg.header.frame_id = map
for (std::size_t idx = 0; idx path_smooth- getStateCount (); idx++)
{
// cast the abstract state type to the type we expect
const ob::SE3StateSpace::StateType *se3state = path_smooth- getState(idx)- as ob::SE3StateSpace::StateType
// extract the first component of the state and cast it to what we expect
const ob::RealVectorStateSpace::StateType *pos = se3state- as ob::RealVectorStateSpace::StateType
// extract the second component of the state and cast it to what we expect
const ob::SO3StateSpace::StateType *rot = se3state- as ob::SO3StateSpace::StateType
geometry_msgs::PoseStamped point;
// pose.header.frame_id = /world
point.pose.position.x = pos- values[0];
point.pose.position.y = pos- values[1];
point.pose.position.z = pos- values[2];
point.pose.orientation.x = rot-
point.pose.orientation.y = rot-
point.pose.orientation.z = rot-
point.pose.orientation.w = rot-
smooth_msg.poses.push_back(point);
std::cout Published marker: idx std::endl;
vis_pub.publish(smooth_msg);
// ros::Duration(0.1).sleep();
// Clear memory
pdef- clearSolutionPaths();
replan_flag = false;
}
else
std::cout No solution found std::endl;
}
private:
// construct the state space we are planning in
ob::StateSpacePtr space;
// construct an instance of space information from this state space
ob::SpaceInformationPtr si;
// create a problem instance
ob::ProblemDefinitionPtr pdef;
og::PathGeometric* path_smooth;
bool replan_flag = false;
std::shared_ptr fcl::CollisionGeometry Quadcopter;
std::shared_ptr fcl::CollisionGeometry tree_obj;
bool isStateValid(const ob::State *state)
{
// cast the abstract state type to the type we expect
const ob::SE3StateSpace::StateType *se3state = state- as ob::SE3StateSpace::StateType
// extract the first component of the state and cast it to what we expect
const ob::RealVectorStateSpace::StateType *pos = se3state- as ob::RealVectorStateSpace::StateType
// extract the second component of the state and cast it to what we expect
const ob::SO3StateSpace::StateType *rot = se3state- as ob::SO3StateSpace::StateType
fcl::CollisionObject treeObj((tree_obj));
fcl::CollisionObject aircraftObject(Quadcopter);
// check validity of state defined by pos rot
fcl::Vec3f translation(pos- values[0],pos- values[1],pos- values[2]);
fcl::Quaternion3f rotation(rot- w, rot- x, rot- y, rot-
aircraftObject.setTransform(rotation, translation);
fcl::CollisionRequest requestType(1,false,1,false);
fcl::CollisionResult collisionResult;
fcl::collide( aircraftObject, treeObj, requestType, collisionResult);
return(!collisionResult.isCollision());
// Returns a structure representing the optimization objective to use
// for optimal motion planning. This method returns an objective which
// attempts to minimize the length in configuration space of computed
// paths.
ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr si)
{
ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
// obj- setCostThreshold(ob::Cost(1.51));
return obj;
ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr si)
{
ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
obj- setCostToGoHeuristic( ob::goalRegionCostToGo);
return obj;
void octomapCallback(const octomap_msgs::Octomap::ConstPtr msg, planner* planner_ptr)
{
//loading octree from binary
// const std::string filename = /home/xiaopeng/dense.bt
// octomap::OcTree temp_tree(0.1);
// temp_tree.readBinary(filename);
// fcl::OcTree* tree = new fcl::OcTree(std::shared_ptr const octomap::OcTree ( temp_tree));
// convert octree to collision object
octomap::OcTree* tree_oct = dynamic_cast octomap::OcTree* (octomap_msgs::msgToMap(*msg));
fcl::OcTree* tree = new fcl::OcTree(std::shared_ptr const octomap::OcTree (tree_oct));
// Update the octree used for collision checking
planner_ptr- updateMap(std::shared_ptr fcl::CollisionGeometry (tree));
planner_ptr- replan();
void odomCb(const nav_msgs::Odometry::ConstPtr msg, planner* planner_ptr)
{
planner_ptr- setStart(msg- pose.pose.position.x, msg- pose.pose.position.y, msg- pose.pose.position.z);
void startCb(const geometry_msgs::PointStamped::ConstPtr msg, planner* planner_ptr)
{
planner_ptr- setStart(msg- point.x, msg- point.y, msg- point.z);
void goalCb(const geometry_msgs::PointStamped::ConstPtr msg, planner* planner_ptr)
{
planner_ptr- setGoal(msg- point.x, msg- point.y, msg- point.z);
planner_ptr- plan();
int main(int argc, char **argv)
{
ros::init(argc, argv, octomap_planner
ros::NodeHandle n;
planner planner_object;
ros::Subscriber octree_sub = n.subscribe octomap_msgs::Octomap ( /octomap_binary , 1, boost::bind( octomapCallback, _1, planner_object));
// ros::Subscriber odom_sub = n.subscribe nav_msgs::Odometry ( /rovio/odometry , 1, boost::bind( odomCb, _1, planner_object));
ros::Subscriber goal_sub = n.subscribe geometry_msgs::PointStamped ( /goal/clicked_point , 1, boost::bind( goalCb, _1, planner_object));
ros::Subscriber start_sub = n.subscribe geometry_msgs::PointStamped ( /start/clicked_point , 1, boost::bind( startCb, _1, planner_object));
// vis_pub = n.advertise visualization_msgs::Marker ( visualization_marker , 0 );
vis_pub = n.advertise nav_msgs::Path ( visualization_marker , 0 );
// traj_pub = n.advertise trajectory_msgs::MultiDOFJointTrajectory ( waypoints ,1);
traj_pub = n.advertise nav_msgs::Path ( waypoints ,1);
std::cout OMPL version: OMPL_VERSION std::endl;
ros::spin();
return 0;
}