SE2RigidBodyPlanning.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #include <omplapp/apps/SE2RigidBodyPlanning.h>
14 #include <omplapp/config.h>
15 
16 using namespace ompl;
17 
18 int main()
19 {
20  // plan in SE2
22 
23  // load the robot and the environment
24  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/car1_planar_robot.dae";
25  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/Maze_planar_env.dae";
26  setup.setRobotMesh(robot_fname);
27  setup.setEnvironmentMesh(env_fname);
28 
29  // define starting state
31  start->setX(0.0);
32  start->setY(0.0);
33 
34  // define goal state
36  goal->setX(26.0);
37  goal->setY(0.0);
38 
39  // set the start & goal states
40  setup.setStartAndGoalStates(start, goal);
41 
42  // attempt to solve the problem, and print it to screen if a solution is found
43  if (setup.solve())
44  setup.getSolutionPath().print(std::cout);
45 
46  return 0;
47 }
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
Definition of a scoped state.
Definition: ScopedState.h:56
void print(std::ostream &out) const override
Print the path to a stream.
void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, double threshold=std::numeric_limits< double >::epsilon())
Set the start and goal states to use.
Definition: SimpleSetup.cpp:90
const base::SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: SimpleSetup.h:75
virtual bool setEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing the environment (env). Returns 1 on success, 0 on failure.
Wrapper for ompl::app::RigidBodyPlanning that plans for rigid bodies in SE2.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
virtual bool setRobotMesh(const std::string &robot)
This function specifies the name of the CAD file representing the robot (robot). Returns 1 on success...