Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7248] pkg/trunk
|
From: <pof...@us...> - 2008-11-25 02:03:55
|
Revision: 7248
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7248&view=rev
Author: poftwaresatent
Date: 2008-11-25 02:03:52 +0000 (Tue, 25 Nov 2008)
Log Message:
-----------
Incorporating Max's recent commits into sbpl_util and highlevel_controllers.
* Added "generic" waypoint list and conversion from SBPLPlanner's raw
state ID vector plans. The conversion also computes some overall
plan "quality measures" in case you need them:
* cumulated plan length
* cumulated tangential direction change
* cumulated theta change
* Extended ompl::SBPLPlannerStatistics and
ompl::SBPLPlannerManager::replan() with:
* bool stop_at_first_solution
* bool plan_from_scratch
* int solution_cost
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/motion_planning/sbpl_util/CMakeLists.txt
pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp
pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh
Added Paths:
-----------
pkg/trunk/motion_planning/sbpl_util/src/plan_wrap.cpp
pkg/trunk/motion_planning/sbpl_util/src/plan_wrap.h
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-25 01:51:45 UTC (rev 7247)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-25 02:03:52 UTC (rev 7248)
@@ -42,6 +42,9 @@
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/basic_observation_buffer.h>
+// Generic OMPL plan representation (will move into <sbpl_util/...> or <ompl_tools/...> later)
+#include <plan_wrap.h>
+
// Message structures used
#include <std_msgs/Planner2DState.h>
#include <std_msgs/Planner2DGoal.h>
@@ -107,8 +110,14 @@
* @see publishPlan
*/
void updatePlan(const std::list<std_msgs::Pose2DFloat32>& newPlan);
-
+
/**
+ * @brief Overwrites the current plan with a new one. Will handle suitable publication
+ * @see publishPlan
+ */
+ void updatePlan(ompl::waypoint_plan_t const & newPlan);
+
+ /**
* @brief test the current plan for collisions with obstacles
*/
bool inCollision() const;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-25 01:51:45 UTC (rev 7247)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-25 02:03:52 UTC (rev 7248)
@@ -41,6 +41,8 @@
#include <std_msgs/Polyline2D.h>
#include <std_srvs/StaticMap.h>
#include <std_msgs/PointStamped.h>
+#include <algorithm>
+#include <iterator>
namespace ros {
namespace highlevel_controllers {
@@ -417,6 +419,16 @@
unlock();
}
+
+ /** \todo Some code duplication wrt MoveBase::updatePlan(const std::list<std_msgs::Pose2DFloat32>&). */
+ void MoveBase::updatePlan(ompl::waypoint_plan_t const & newPlan) {
+ sentry<MoveBase> guard(this);
+ if (!isValid() || plan_.size() > newPlan.size()){
+ plan_.clear();
+ std::copy(newPlan.begin(), newPlan.end(), std::back_inserter(plan_));
+ publishPath(true, plan_);
+ }
+ }
/**
* This is used as a validation check and is only called from within dispatchCommands where the lock has already been
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-25 01:51:45 UTC (rev 7247)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-25 02:03:52 UTC (rev 7248)
@@ -66,6 +66,7 @@
#include <MoveBase.hh>
#include <sbpl_util.hh>
#include <environment_wrap.h>
+#include <plan_wrap.h>
//sbpl headers file
#include <headers.h>
@@ -338,49 +339,29 @@
// Invoke the planner, updating the statistics in the process.
std::vector<int> solutionStateIDs;
statsEntry.allocated_time_sec = plannerTimeLimit_;
- statsEntry.status = pMgr_->replan(statsEntry.allocated_time_sec,
+ statsEntry.stop_at_first_solution = false;
+ statsEntry.plan_from_scratch = false;
+ statsEntry.status = pMgr_->replan(statsEntry.stop_at_first_solution,
+ statsEntry.plan_from_scratch,
+ statsEntry.allocated_time_sec,
&statsEntry.actual_time_wall_sec,
&statsEntry.actual_time_user_sec,
&statsEntry.actual_time_system_sec,
+ &statsEntry.solution_cost,
&solutionStateIDs);
-#warning 'use the (upcoming) sbpl_util/plan_wrap.h for plan conversion'
// Extract the solution, if available, and update statistics (as usual).
statsEntry.plan_length_m = 0;
statsEntry.plan_angle_change_rad = 0;
if ((1 == statsEntry.status) && (1 < solutionStateIDs.size())) {
- std::list<std_msgs::Pose2DFloat32> plan;
- double prevx(0), prevy(0), prevth(0);
- prevth = 42.17; // to detect when it has been initialized (see 42 below)
- for(std::vector<int>::const_iterator it = solutionStateIDs.begin(); it != solutionStateIDs.end(); ++it){
- std_msgs::Pose2DFloat32 const waypoint(env_->GetPoseFromState(*it));
-
- // update stats:
- // - first round, nothing to do
- // - second round, update path length only
- // - third round, update path length and angular change
- if (plan.empty()) {
- prevx = waypoint.x;
- prevy = waypoint.y;
- }
- else {
- double const dx(waypoint.x - prevx);
- double const dy(waypoint.y - prevy);
- statsEntry.plan_length_m += sqrt(pow(dx, 2) + pow(dy, 2));
- double const th(atan2(dy, dx));
- if (42 > prevth) // see 42.17 above
- statsEntry.plan_angle_change_rad += fabs(mod2pi(th - prevth));
- prevx = waypoint.x;
- prevy = waypoint.y;
- prevth = th;
- }
-
- plan.push_back(waypoint);
- }
- // probably we should add the delta from the last theta to
- // the goal theta onto statsEntry.plan_angle_change_rad here, but
- // that depends on whether our planner handles theta for us,
- // and needs special handling if we have no plan...
+ ompl::waypoint_plan_t plan;
+ ompl::convertPlan(*env_,
+ solutionStateIDs,
+ &plan,
+ &statsEntry.plan_length_m,
+ &statsEntry.plan_angle_change_rad,
+ 0 // should be non-null for 3DKIN planning...
+ );
{
ostringstream prefix_os;
prefix_os << "[" << goalCount_ << "] ";
Modified: pkg/trunk/motion_planning/sbpl_util/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/CMakeLists.txt 2008-11-25 01:51:45 UTC (rev 7247)
+++ pkg/trunk/motion_planning/sbpl_util/CMakeLists.txt 2008-11-25 02:03:52 UTC (rev 7248)
@@ -14,7 +14,8 @@
src/sbpl_util.cpp
src/costmap_wrap.cpp
src/footprint.cpp
- src/environment_wrap.cpp)
+ src/environment_wrap.cpp
+ src/plan_wrap.cpp)
target_link_libraries (sbpl_util costmap_2d)
rospack_add_executable (t3dobst src/t3dobst.cpp)
Added: pkg/trunk/motion_planning/sbpl_util/src/plan_wrap.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/plan_wrap.cpp (rev 0)
+++ pkg/trunk/motion_planning/sbpl_util/src/plan_wrap.cpp 2008-11-25 02:03:52 UTC (rev 7248)
@@ -0,0 +1,90 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "plan_wrap.h"
+#include "environment_wrap.h"
+#include <sfl/util/numeric.hpp>
+
+namespace ompl {
+
+ void convertPlan(EnvironmentWrapper const & environment,
+ raw_sbpl_plan_t const & raw,
+ waypoint_plan_t * plan,
+ double * optPlanLengthM,
+ double * optTangentChangeRad,
+ double * optDirectionChangeRad)
+ {
+ double tmpPlanLengthM;
+ double tmpTangentChangeRad;
+ double tmpDirectionChangeRad;
+ double * planLengthM(optPlanLengthM ? optPlanLengthM : &tmpPlanLengthM);
+ double * tangentChangeRad(optTangentChangeRad ? optTangentChangeRad : &tmpTangentChangeRad);
+ double * directionChangeRad(optDirectionChangeRad ? optDirectionChangeRad : &tmpDirectionChangeRad);
+ *planLengthM = 0;
+ *tangentChangeRad = 0;
+ *directionChangeRad = 0;
+
+ double prevx(0), prevy(0), prevtan(0), prevdir(0);
+ prevtan = 42.17; // to detect when it has been initialized (see 42 below)
+ for (raw_sbpl_plan_t::const_iterator it(raw.begin()); it != raw.end(); ++it) {
+ std_msgs::Pose2DFloat32 const waypoint(environment.GetPoseFromState(*it));
+
+ // update stats:
+ // - first round, nothing to do
+ // - second round, update path length only
+ // - third round, update path length and angular change
+ if (plan->empty()) {
+ prevx = waypoint.x;
+ prevy = waypoint.y;
+ prevdir = waypoint.th;
+ }
+ else {
+ double const dx(waypoint.x - prevx);
+ double const dy(waypoint.y - prevy);
+ *planLengthM += sqrt(pow(dx, 2) + pow(dy, 2));
+ *directionChangeRad += fabs(sfl::mod2pi(waypoint.th - prevdir));
+ double const tangent(atan2(dy, dx));
+ if (42 > prevtan) // see 42.17 above
+ *tangentChangeRad += fabs(sfl::mod2pi(tangent - prevtan));
+ prevx = waypoint.x;
+ prevy = waypoint.y;
+ prevdir = waypoint.th;
+ prevtan = tangent;
+ }
+
+ plan->push_back(waypoint);
+ }
+ }
+
+}
Added: pkg/trunk/motion_planning/sbpl_util/src/plan_wrap.h
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/plan_wrap.h (rev 0)
+++ pkg/trunk/motion_planning/sbpl_util/src/plan_wrap.h 2008-11-25 02:03:52 UTC (rev 7248)
@@ -0,0 +1,76 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#ifndef OMPL_PLAN_WRAP_HPP
+#define OMPL_PLAN_WRAP_HPP
+
+#include <std_msgs/Pose2DFloat32.h>
+#include <vector>
+
+namespace ompl {
+
+ typedef std::vector<int> raw_sbpl_plan_t;
+ typedef std::vector<std_msgs::Pose2DFloat32> waypoint_plan_t;
+
+ class EnvironmentWrapper;
+
+ /**
+ Convert a plan from a raw state ID sequence (as computed by
+ SBPLPlanner subclasses) to waypoints that are
+ understandable. Optionally provides some statistics on the plan.
+ */
+ void convertPlan(/** in: how to translate state IDs to std_msgs::Pose2DFloat32 */
+ EnvironmentWrapper const & environment,
+ /** in: the raw plan */
+ raw_sbpl_plan_t const & raw,
+ /** out: the converted plan (it is just appended
+ to, not cleared for you) */
+ waypoint_plan_t * plan,
+ /** optional out: the cumulated path length */
+ double * optPlanLengthM,
+ /** optional out: the cumulated change in the path
+ tangential direction (the angle between the
+ x-axis and the delta between two successive
+ waypoints) */
+ double * optTangentChangeRad,
+ /** optional out: the cumulated change in the
+ direction of the waypoints (the delta of
+ std_msgs::Pose2DFloat32::th values).
+ \note This only makes sense for plans that are
+ aware of the robot's heading though. */
+ double * optDirectionChangeRad);
+
+}
+
+#endif // OMPL_PLAN_WRAP_HPP
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-11-25 01:51:45 UTC (rev 7247)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-11-25 02:03:52 UTC (rev 7248)
@@ -157,22 +157,33 @@
int SBPLPlannerManager::
- replan(double allocated_time_sec,
+ replan(bool stop_at_first_solution,
+ bool plan_from_scratch,
+ double allocated_time_sec,
double * actual_time_wall_sec,
double * actual_time_user_sec,
double * actual_time_system_sec,
+ int * solution_cost,
vector<int>* solution_stateIDs_V) throw(no_planner_selected)
{
if ( ! planner_)
throw no_planner_selected();
+ if (stop_at_first_solution)
+ planner_->set_search_mode(true);
+ else
+ planner_->set_search_mode(false);
+
+ if (plan_from_scratch)
+ force_planning_from_scratch();
+
struct rusage ru_started;
struct timeval t_started;
getrusage(RUSAGE_SELF, &ru_started);
gettimeofday(&t_started, 0);
+
+ int const status(planner_->replan(allocated_time_sec, solution_stateIDs_V, solution_cost));
- int const status(planner_->replan(allocated_time_sec, solution_stateIDs_V));
-
struct rusage ru_finished;
struct timeval t_finished;
getrusage(RUSAGE_SELF, &ru_finished);
@@ -263,22 +274,9 @@
void SBPLPlannerStatistics::entry::
logInfo(char const * prefix) const
{
- ROS_INFO("%s\n"
- "%splanner: %s\n"
- "%sgoal (map/grid/state): %+8.3f %+8.3f %+8.3f / %u %u / %d\n"
- "%sstart (map/grid/state): %+8.3f %+8.3f %+8.3f / %u %u / %d\n"
- "%stime [s] (actual/alloc): %g / %g\n"
- "%sstatus (1 == SUCCESS): %d\n"
- "%splan_length [m]: %+8.3f\n"
- "%splan_rotation [rad]: %+8.3f\n",
- prefix,
- prefix, plannerType.c_str(),
- prefix, goal.x, goal.y, goal.th, goalIx, goalIy, goalState,
- prefix, start.x, start.y, start.th, startIx, startIy, startState,
- prefix, actual_time_wall_sec, allocated_time_sec,
- prefix, status,
- prefix, plan_length_m,
- prefix, plan_angle_change_rad);
+ ostringstream os;
+ logStream(os, "ompl::SBPLPlannerStatistics", prefix);
+ ROS_INFO("%s", os.str().c_str());
}
@@ -313,10 +311,13 @@
<< prefix << "start map: " << start.x << " " << start.y << " " << start.th << "\n"
<< prefix << "start grid: " << startIx << " " << startIy << "\n"
<< prefix << "start state: " << startState << "\n"
+ << prefix << "stop at first solution:" << (stop_at_first_solution ? "true\n" : "false\n")
+ << prefix << "plan from scratch: " << (plan_from_scratch ? "true\n" : "false\n")
<< prefix << "time alloc: " << allocated_time_sec << "\n"
<< prefix << "time actual (wall): " << actual_time_wall_sec << "\n"
<< prefix << "time actual (user): " << actual_time_user_sec << "\n"
<< prefix << "time actual (system): " << actual_time_system_sec << "\n"
+ << prefix << "solution cost: " << solution_cost << "\n"
<< prefix << "status (1 == SUCCESS): " << status << "\n"
<< prefix << "plan_length [m]: " << plan_length_m << "\n"
<< prefix << "plan_rotation [rad]: " << plan_angle_change_rad << "\n";
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-11-25 01:51:45 UTC (rev 7247)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-11-25 02:03:52 UTC (rev 7248)
@@ -112,12 +112,15 @@
unsigned int startIx; /**< x-index of the start in the costmap */
unsigned int startIy; /**< y-index of the start in the costmap */
int startState; /**< stateID of the start (from costmap indices) */
+ bool stop_at_first_solution; /**< whether to just plan until any plan is found */
+ bool plan_from_scratch; /**< whether to discard any previous solutions */
double allocated_time_sec; /**< the amount of time we had available for planning */
double actual_time_wall_sec; /**< the amount of time actually used for planning (wallclock) */
double actual_time_user_sec; /**< the amount of time actually used for planning (user time) */
double actual_time_system_sec; /**< the amount of time actually used for planning (system time) */
int status; /**< return value of replan() (i.e. success == 1, or -42 if replan() never got called) */
+ int solution_cost; /**< cost of the solution, as given by replan() */
double plan_length_m; /**< cumulated Euclidean distance between planned waypoints */
double plan_angle_change_rad; /**< cumulated abs(delta(angle)) along planned waypoints */
@@ -195,11 +198,28 @@
/** Dispatch to the currently select()-ed planner's
SBPLPlanner::replan(), measuring the time it actually takes to
- run it. */
- int replan(double allocated_time_sec,
+ run it.
+
+ \note You can use ompl::convertPlan() to translate the
+ solution_stateIDs_V into something more generic.
+
+ \return The status returned by SBPLPlanner::replan().
+ */
+ int replan(/** in: whether to just return the first feasible solution */
+ bool stop_at_first_solution,
+ /** in: whether to forcefully re-initialize the planner */
+ bool plan_from_scratch,
+ /** in: how much time is allocated for planning */
+ double allocated_time_sec,
+ /** out: how much time was actually used by the planner (wallclock) */
double * actual_time_wall_sec,
+ /** out: how much time was actually used by the planner (user time) */
double * actual_time_user_sec,
+ /** out: how much time was actually used by the planner (system time) */
double * actual_time_system_sec,
+ /** out: the cost of the planned path */
+ int * solution_cost,
+ /** out: the planned path, as a succession of state IDs */
std::vector<int>* solution_stateIDs_V) throw(no_planner_selected);
/** Dispatch to the currently select()-ed planner's
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
