Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3013] pkg/trunk
|
From: <is...@us...> - 2008-08-13 22:37:24
|
Revision: 3013
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3013&view=rev
Author: isucan
Date: 2008-08-13 22:37:32 +0000 (Wed, 13 Aug 2008)
Log Message:
-----------
created a package with base classes for nodes that need robot models. also updated ompl to nicer structs for specifying distance functions and state validity checkers
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
pkg/trunk/world_models/world_3d_map/manifest.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_node_util/
pkg/trunk/motion_planning/planning_node_util/CMakeLists.txt
pkg/trunk/motion_planning/planning_node_util/Makefile
pkg/trunk/motion_planning/planning_node_util/include/
pkg/trunk/motion_planning/planning_node_util/include/knode.h
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/motion_planning/planning_node_util/manifest.xml
Removed Paths:
-------------
pkg/trunk/motion_planning/planning_models/include/planning_models/node.h
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-13 21:31:23 UTC (rev 3012)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-13 22:37:32 UTC (rev 3013)
@@ -287,7 +287,7 @@
/* cleanup */
p.si->clearGoal();
- p.si->clearStartStates(true);
+ p.si->clearStartStates();
p.mp->clear();
return true;
@@ -371,11 +371,14 @@
}
private:
-
+
+ class StateValidityPredicate;
+
struct Planner
{
ompl::Planner_t mp;
ompl::SpaceInformationKinematic_t si;
+ StateValidityPredicate* svc;
int type;
};
@@ -403,34 +406,52 @@
planning_models::KinematicModel *kmodel;
int groupID;
};
-
- std_msgs::PointCloudFloat32 m_cloud;
- std_msgs::RobotBase2DOdom m_localizedPose;
- double m_basePos[3];
- collision_space::EnvironmentModel *m_collisionSpace;
- std::map<std::string, Model*> m_models;
- std::vector<robot_desc::URDF*> m_robotDescriptions;
-
- static bool isStateValid(const ompl::SpaceInformationKinematic::StateKinematic_t state, void *data)
+ class StateValidityPredicate : public ompl::SpaceInformation::StateValidityChecker
{
- Model *model = reinterpret_cast<Model*>(data);
- model->kmodel->computeTransforms(state->values, model->groupID);
- model->collisionSpace->updateRobotModel(model->collisionSpaceID);
- bool collision = model->collisionSpace->isCollision(model->collisionSpaceID);
- return !collision;
- }
+ public:
+ StateValidityPredicate(Model *model) : ompl::SpaceInformation::StateValidityChecker()
+ {
+ m_model = model;
+ }
+
+ virtual bool operator()(const ompl::SpaceInformation::State_t state)
+ {
+ m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
+ m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
+ bool collision = m_model->collisionSpace->isCollision(m_model->collisionSpaceID);
+ return !collision;
+ }
+
+ protected:
+
+ Model *m_model;
+ };
+
void createMotionPlanningInstances(Model* model)
{
+ addRRTInstance(model);
+ }
+
+ void addRRTInstance(Model *model)
+ {
Planner p;
p.si = new SpaceInformationXMLModel(model->kmodel, model->groupID);
- p.si->setStateValidFn(isStateValid, reinterpret_cast<void*>(model));
+ p.svc = new StateValidityPredicate(model);
+ p.si->setStateValidityChecker(p.svc);
p.mp = new ompl::RRT(p.si);
- p.type = 0;
+ p.type = 0;
model->planners.push_back(p);
}
-
+
+ std_msgs::PointCloudFloat32 m_cloud;
+ std_msgs::RobotBase2DOdom m_localizedPose;
+ double m_basePos[3];
+ collision_space::EnvironmentModel *m_collisionSpace;
+ std::map<std::string, Model*> m_models;
+ std::vector<robot_desc::URDF*> m_robotDescriptions;
+
};
int main(int argc, char **argv)
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2008-08-13 21:31:23 UTC (rev 3012)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2008-08-13 22:37:32 UTC (rev 3013)
@@ -41,40 +41,139 @@
namespace ompl
{
+ /** Forward class declaration */
ForwardClassDeclaration(SpaceInformation);
class SpaceInformation
{
public:
+
+ /** Constructor */
SpaceInformation(void)
{
- m_goal = NULL;
+ m_goal = NULL;
+ m_setup = false;
+ m_stateDistanceEvaluator = NULL;
+ m_stateValidityChecker = NULL;
}
+ /** Destructor */
virtual ~SpaceInformation(void)
{
}
+ /************************************************************/
+ /* States */
+ /************************************************************/
+ /** Forward class declaration */
+ ForwardClassDeclaration(State);
+
+ /** Abstract definition of a state */
+ class State
+ {
+ public:
+ virtual ~State(void)
+ {
+ }
+ };
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(StateValidityChecker);
+
+ /** Abstract definition for a class checking the validity of states */
+ class StateValidityChecker
+ {
+ public:
+ /** Return true if the state is valid */
+ virtual bool operator()(const State_t state) = 0;
+ };
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(StateDistanceEvaluator);
+
+ /** Abstract definition for a class evaluating distance between states */
+ class StateDistanceEvaluator
+ {
+ public:
+ /** Return true if the state is valid */
+ virtual double operator()(const State_t state1, const State_t state2) = 0;
+ };
+
+ /** Add a start state */
+ void addStartState(State_t state)
+ {
+ m_startStates.push_back(state);
+ }
+
+ /** Clear all start states (memory is freed) */
+ void clearStartStates(void)
+ {
+ for (unsigned int i = 0 ; i < m_startStates.size() ; ++i)
+ delete m_startStates[i];
+ m_startStates.clear();
+ }
+
+ /** Clear all start states but do not free memory */
+ void forgetStartStates(void)
+ {
+ m_startStates.clear();
+ }
+
+ /** Returns the number of start states */
+ unsigned int getStartStateCount(void) const
+ {
+ return m_startStates.size();
+ }
+
+ /** Returns a specific start state */
+ State_t getStartState(unsigned int index) const
+ {
+ return m_startStates[index];
+ }
+
+ /** Set the instance of the distance evaluator to use. This is
+ only needed by some planning algorithms. No memory freeing is performed. */
+ void setDistanceEvaluator(StateDistanceEvaluator *sde)
+ {
+ m_stateDistanceEvaluator = sde;
+ }
+
+ /** Set the instance of the validity checker to use. No memory freeing is performed. */
+ void setStateValidityChecker(StateValidityChecker *svc)
+ {
+ m_stateValidityChecker = svc;
+ }
+
/************************************************************/
/* Paths */
/************************************************************/
+ /** Forward declaration */
ForwardClassDeclaration(Path);
+ /** Abstract definition of a path */
class Path
{
public:
+ /** Constructor. A path must always know the space information it is part of */
Path(SpaceInformation_t si)
{
m_si = si;
}
-
+
+ /** Destructor */
virtual ~Path(void)
{
}
+ /** Returns the space information this path is part of */
+ SpaceInformation_t getSpaceInformation(void) const
+ {
+ return m_si;
+ }
+
protected:
SpaceInformation_t m_si;
@@ -86,12 +185,15 @@
/* Goals */
/************************************************************/
+ /** Forward declaration */
ForwardClassDeclaration(Goal);
+ /** Abstract definition of goals. Will contain solutions, if found */
class Goal
{
public:
-
+
+ /** Constructor. The goal must always know the space information it is part of */
Goal(SpaceInformation_t si)
{
m_si = si;
@@ -100,38 +202,66 @@
m_approximate = false;
}
+ /** Destructor. Clears the solution as well */
virtual ~Goal(void)
{
if (m_path)
delete m_path;
}
+
+ /** Returns the space information this goal is part of */
+ SpaceInformation_t getSpaceInformation(void) const
+ {
+ return m_si;
+ }
+ /** Returns true if a solution path has been found */
bool isAchieved(void) const
{
return m_path != NULL;
}
+ /** Return the found solution path */
Path_t getSolutionPath(void) const
{
return m_path;
}
+ /** Forget the solution path. Memory is not freed. This is
+ useful when the user wants to keep the solution path
+ but wants to clear the goal. The user takes
+ responsibilty to free the memory for the solution
+ path. */
+ void forgetSolutionPath(void)
+ {
+ m_path = NULL;
+ }
+
+ /** Update the solution path. If a previous solution path exists, it is deleted. */
void setSolutionPath(Path_t path, bool approximate = false)
{
+ if (m_path)
+ delete m_path;
m_path = path;
m_approximate = approximate;
}
+ /** If a difference between the desired solution and the
+ solution found is computed by the planner, this functions
+ returns it */
double getDifference(void) const
{
return m_difference;
}
+ /** Set the difference between the found solution path and
+ the desired solution path */
void setDifference(double difference)
{
m_difference = difference;
}
+ /** Return true if the found solution is approximate */
bool isApproximate(void) const
{
return m_approximate;
@@ -153,6 +283,8 @@
};
+
+ /** Set the goal. The memory for a previous goal is freed. */
void setGoal(Goal_t goal)
{
if (m_goal)
@@ -160,6 +292,7 @@
m_goal = goal;
}
+ /** Clear the goal. Memory is freed. */
void clearGoal(bool free = true)
{
if (free && m_goal)
@@ -167,54 +300,36 @@
m_goal = NULL;
}
+ /** Return the current goal */
Goal_t getGoal(void) const
{
return m_goal;
}
+ /** Clear the goal, but do not free its memory */
+ void forgetGoal(void)
+ {
+ m_goal = NULL;
+ }
/************************************************************/
- /* States */
+ /* Utility functions */
/************************************************************/
-
- ForwardClassDeclaration(State);
- class State
+ /** Perform additional setup tasks (run once, before use) */
+ virtual void setup(void)
{
- public:
- virtual ~State(void)
- {
- }
- };
-
- void addStartState(State_t state)
- {
- m_startStates.push_back(state);
+ m_setup = true;
}
- void clearStartStates(bool free = true)
- {
- if (free)
- for (unsigned int i = 0 ; i < m_startStates.size() ; ++i)
- delete m_startStates[i];
- m_startStates.clear();
- }
-
- unsigned int getStartStateCount(void) const
- {
- return m_startStates.size();
- }
+ protected:
- State_t getStartState(unsigned int index) const
- {
- return m_startStates[index];
- }
+ bool m_setup;
+ std::vector<State_t> m_startStates;
+ Goal_t m_goal;
+ StateValidityChecker *m_stateValidityChecker;
+ StateDistanceEvaluator *m_stateDistanceEvaluator;
- protected:
-
- Goal_t m_goal;
- std::vector<State_t> m_startStates;
-
};
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-08-13 21:31:23 UTC (rev 3012)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-08-13 22:37:32 UTC (rev 3013)
@@ -40,31 +40,38 @@
namespace ompl
{
-
+
+ /** Forward class declaration */
ForwardClassDeclaration(SpaceInformationKinematic);
+ /** Space information useful for kinematic planning */
class SpaceInformationKinematic : public SpaceInformation
{
public:
- SpaceInformationKinematic() : SpaceInformation()
+ /** Constructor; setup() needs to be called as well, before use */
+ SpaceInformationKinematic(void) : SpaceInformation(),
+ m_defaultDistanceEvaluator(this)
{
random_utils::init(&m_rngState);
-
- m_isValidStateFn = NULL;
- m_isValidStateFnData = NULL;
+ m_stateDistanceEvaluator = &m_defaultDistanceEvaluator;
m_smoother.rangeRatio = 0.2;
m_smoother.maxSteps = 10;
m_smoother.maxEmptySteps = 3;
}
+ /** Destructor */
virtual ~SpaceInformationKinematic(void)
{
}
+
ForwardClassDeclaration(StateKinematic);
-
+ ForwardClassDeclaration(GoalRegionKinematic);
+ ForwardClassDeclaration(GoalStateKinematic);
+ ForwardClassDeclaration(PathKinematic);
+
class StateKinematic : public State
{
public:
@@ -88,8 +95,6 @@
double *values;
};
- ForwardClassDeclaration(GoalRegionKinematic);
-
class GoalRegionKinematic : public Goal
{
public:
@@ -108,7 +113,6 @@
double threshold;
};
- ForwardClassDeclaration(GoalStateKinematic);
class GoalStateKinematic : public GoalRegionKinematic
{
@@ -133,7 +137,6 @@
StateKinematic_t state;
};
- ForwardClassDeclaration(PathKinematic);
class PathKinematic : public Path
{
@@ -159,6 +162,21 @@
}
};
+ class StateKinematicL2SquareDistanceEvaluator : public StateDistanceEvaluator
+ {
+ public:
+ StateKinematicL2SquareDistanceEvaluator(SpaceInformationKinematic_t si) : StateDistanceEvaluator()
+ {
+ m_si = si;
+ }
+
+ virtual double operator()(const State_t state1, const State_t state2);
+
+ protected:
+
+ SpaceInformationKinematic_t m_si;
+ };
+
struct StateComponent
{
StateComponent(void)
@@ -173,15 +191,7 @@
double maxValue;
double resolution;
};
-
- typedef bool (*IsStateValidFn)(const StateKinematic_t, void*);
-
- void setStateValidFn(IsStateValidFn fun, void *data)
- {
- m_isValidStateFn = fun;
- m_isValidStateFnData = data;
- }
-
+
virtual void printState(const StateKinematic_t state, FILE* out = stdout) const;
virtual void copyState(StateKinematic_t destination, const StateKinematic_t source)
{
@@ -198,25 +208,34 @@
return m_stateComponent[index];
}
- virtual double distance(const StateKinematic_t s1, const StateKinematic_t s2);
-
+ double distance(const StateKinematic_t s1, const StateKinematic_t s2)
+ {
+ return (*m_stateDistanceEvaluator)(static_cast<const State_t>(s1), static_cast<const State_t>(s2));
+ }
+
virtual void sample(StateKinematic_t state);
virtual void sampleNear(StateKinematic_t state, const StateKinematic_t near, double rho);
virtual void smoothVertices(PathKinematic_t path);
virtual bool checkMotion(const StateKinematic_t s1, const StateKinematic_t s2);
- virtual bool isValid(const StateKinematic_t state)
+
+ bool isValid(const StateKinematic_t state)
{
- return m_isValidStateFn(state, m_isValidStateFnData);
+ return (*m_stateValidityChecker)(static_cast<const State_t>(state));
}
virtual void printSettings(FILE *out = stdout) const;
+ virtual void setup(void)
+ {
+ assert(m_stateValidityChecker);
+ SpaceInformation::setup();
+ }
+
protected:
- unsigned int m_stateDimension;
- std::vector<StateComponent> m_stateComponent;
- IsStateValidFn m_isValidStateFn;
- void *m_isValidStateFnData;
+ unsigned int m_stateDimension;
+ std::vector<StateComponent> m_stateComponent;
+ StateKinematicL2SquareDistanceEvaluator m_defaultDistanceEvaluator;
struct
{
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-08-13 21:31:23 UTC (rev 3012)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-08-13 22:37:32 UTC (rev 3013)
@@ -37,25 +37,28 @@
#include <algorithm>
#include <queue>
-void ompl::SpaceInformationKinematic::printState(const StateKinematic_t state, FILE* out) const
+double ompl::SpaceInformationKinematic::StateKinematicL2SquareDistanceEvaluator::operator()(const State_t s1, const State_t s2)
{
- for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
- fprintf(out, "%0.6f ", state->values[i]);
- fprintf(out, "\n");
-}
-
-double ompl::SpaceInformationKinematic::distance(const StateKinematic_t s1, const StateKinematic_t s2)
-{
+ const StateKinematic_t sk1 = static_cast<const StateKinematic_t>(s1);
+ const StateKinematic_t sk2 = static_cast<const StateKinematic_t>(s2);
+ const unsigned int dim = m_si->getStateDimension();
+
double dist = 0.0;
- for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ for (unsigned int i = 0 ; i < dim ; ++i)
{
- double diff = m_stateComponent[i].type == StateComponent::ANGLE ?
- math_utils::shortest_angular_distance(s1->values[i], s2->values[i]) : s1->values[i] - s2->values[i];
+ double diff = m_si->getStateComponent(i).type == StateComponent::ANGLE ?
+ math_utils::shortest_angular_distance(sk1->values[i], sk2->values[i]) : sk1->values[i] - sk2->values[i];
dist += diff * diff;
}
return dist;
}
+void ompl::SpaceInformationKinematic::printState(const StateKinematic_t state, FILE* out) const
+{
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ fprintf(out, "%0.6f ", state->values[i]);
+ fprintf(out, "\n");
+}
void ompl::SpaceInformationKinematic::sample(StateKinematic_t state)
{
for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
Deleted: pkg/trunk/motion_planning/planning_models/include/planning_models/node.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/node.h 2008-08-13 21:31:23 UTC (rev 3012)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/node.h 2008-08-13 22:37:32 UTC (rev 3013)
@@ -1,166 +0,0 @@
-/*********************************************************************
-* 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 <ros/node.h>
-#include <ros/time.h>
-#include <urdf/URDF.h>
-#include <planning_models/kinematic.h>
-#include <std_msgs/RobotBase2DOdom.h>
-#include <rosTF/rosTF.h>
-
-namespace planning_models
-{
-
- class NodeWithRobotModel : public ros::node
- {
-
- public:
-
- NodeWithRobotModel(const std::string &robot_model, const std::string &name, uint32_t options = 0) : ros::node(name, options),
- m_tf(*this, true, 1 * 1000000000ULL, 1000000000ULL)
- {
- m_urdf = NULL;
- m_kmodel = NULL;
- m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
- loadRobotDescription(robot_model);
-
- subscribe("localizedpose", m_localizedPose, &NodeWithRobotModel::localizedPoseCallback);
- }
-
- virtual ~NodeWithRobotModel(void)
- {
- if (m_urdf)
- delete m_urdf;
- if (m_kmodel)
- delete m_kmodel;
- }
-
- void setRobotDescriptionFromData(const char *data)
- {
- robot_desc::URDF *file = new robot_desc::URDF();
- if (file->loadString(data))
- setRobotDescription(file);
- else
- delete file;
- }
-
- void setRobotDescriptionFromFile(const char *filename)
- {
- robot_desc::URDF *file = new robot_desc::URDF();
- if (file->loadFile(filename))
- setRobotDescription(file);
- else
- delete file;
- }
-
- virtual void setRobotDescription(robot_desc::URDF *file)
- {
- if (m_urdf)
- delete m_urdf;
- if (m_kmodel)
- delete m_kmodel;
-
- m_urdf = file;
- m_kmodel = new planning_models::KinematicModel();
- m_kmodel->setVerbose(false);
- m_kmodel->build(*file);
- }
-
- virtual void loadRobotDescription(const std::string &robot_model)
- {
- if (!robot_model.empty() && robot_model != "-")
- {
- std::string content;
- if (get_param(robot_model, content))
- setRobotDescriptionFromData(content.c_str());
- else
- fprintf(stderr, "Robot model '%s' not found!\n", robot_model.c_str());
- }
- }
-
- protected:
-
- void localizedPoseCallback(void)
- {
- bool success = true;
- libTF::TFPose2D pose;
- pose.x = m_localizedPose.pos.x;
- pose.y = m_localizedPose.pos.y;
- pose.yaw = m_localizedPose.pos.th;
- pose.time = m_localizedPose.header.stamp.to_ull();
- pose.frame = m_localizedPose.header.frame_id;
-
- try
- {
- pose = m_tf.transformPose2D("FRAMEID_MAP", pose);
- }
- catch(libTF::TransformReference::LookupException& ex)
- {
- fprintf(stderr, "Discarding pose: Transform reference lookup exception\n");
- success = false;
- }
- catch(libTF::TransformReference::ExtrapolateException& ex)
- {
- fprintf(stderr, "Discarding pose: Extrapolation exception: %s\n", ex.what());
- success = false;
- }
- catch(...)
- {
- fprintf(stderr, "Discarding pose: Exception in pose computation\n");
- success = false;
- }
-
- if (success)
- {
- m_basePos[0] = pose.x;
- m_basePos[1] = pose.y;
- m_basePos[2] = pose.yaw;
-
- baseUpdate();
- }
- }
-
- virtual void baseUpdate(void)
- {
- }
-
- rosTFClient m_tf;
- std_msgs::RobotBase2DOdom m_localizedPose;
- robot_desc::URDF *m_urdf;
- planning_models::KinematicModel *m_kmodel;
- double m_basePos[3];
-
- };
-
-}
Copied: pkg/trunk/motion_planning/planning_node_util/CMakeLists.txt (from rev 3011, pkg/trunk/motion_planning/collision_space/CMakeLists.txt)
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/CMakeLists.txt (rev 0)
+++ pkg/trunk/motion_planning/planning_node_util/CMakeLists.txt 2008-08-13 22:37:32 UTC (rev 3013)
@@ -0,0 +1,3 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(planning_node_util)
Copied: pkg/trunk/motion_planning/planning_node_util/Makefile (from rev 3011, pkg/trunk/motion_planning/collision_space/Makefile)
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/Makefile (rev 0)
+++ pkg/trunk/motion_planning/planning_node_util/Makefile 2008-08-13 22:37:32 UTC (rev 3013)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Copied: pkg/trunk/motion_planning/planning_node_util/include/knode.h (from rev 3011, pkg/trunk/motion_planning/planning_models/include/planning_models/node.h)
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/knode.h (rev 0)
+++ pkg/trunk/motion_planning/planning_node_util/include/knode.h 2008-08-13 22:37:32 UTC (rev 3013)
@@ -0,0 +1,166 @@
+/*********************************************************************
+* 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 <ros/node.h>
+#include <ros/time.h>
+#include <urdf/URDF.h>
+#include <planning_models/kinematic.h>
+#include <std_msgs/RobotBase2DOdom.h>
+#include <rosTF/rosTF.h>
+
+namespace planning_node_util
+{
+
+ class NodeWithRobotModel : public ros::node
+ {
+
+ public:
+
+ NodeWithRobotModel(const std::string &robot_model, const std::string &name, uint32_t options = 0) : ros::node(name, options),
+ m_tf(*this, true, 1 * 1000000000ULL, 1000000000ULL)
+ {
+ m_urdf = NULL;
+ m_kmodel = NULL;
+ m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
+ loadRobotDescription(robot_model);
+
+ subscribe("localizedpose", m_localizedPose, &NodeWithRobotModel::localizedPoseCallback);
+ }
+
+ virtual ~NodeWithRobotModel(void)
+ {
+ if (m_urdf)
+ delete m_urdf;
+ if (m_kmodel)
+ delete m_kmodel;
+ }
+
+ void setRobotDescriptionFromData(const char *data)
+ {
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadString(data))
+ setRobotDescription(file);
+ else
+ delete file;
+ }
+
+ void setRobotDescriptionFromFile(const char *filename)
+ {
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadFile(filename))
+ setRobotDescription(file);
+ else
+ delete file;
+ }
+
+ virtual void setRobotDescription(robot_desc::URDF *file)
+ {
+ if (m_urdf)
+ delete m_urdf;
+ if (m_kmodel)
+ delete m_kmodel;
+
+ m_urdf = file;
+ m_kmodel = new planning_models::KinematicModel();
+ m_kmodel->setVerbose(false);
+ m_kmodel->build(*file);
+ }
+
+ virtual void loadRobotDescription(const std::string &robot_model)
+ {
+ if (!robot_model.empty() && robot_model != "-")
+ {
+ std::string content;
+ if (get_param(robot_model, content))
+ setRobotDescriptionFromData(content.c_str());
+ else
+ fprintf(stderr, "Robot model '%s' not found!\n", robot_model.c_str());
+ }
+ }
+
+ protected:
+
+ void localizedPoseCallback(void)
+ {
+ bool success = true;
+ libTF::TFPose2D pose;
+ pose.x = m_localizedPose.pos.x;
+ pose.y = m_localizedPose.pos.y;
+ pose.yaw = m_localizedPose.pos.th;
+ pose.time = m_localizedPose.header.stamp.to_ull();
+ pose.frame = m_localizedPose.header.frame_id;
+
+ try
+ {
+ pose = m_tf.transformPose2D("FRAMEID_MAP", pose);
+ }
+ catch(libTF::TransformReference::LookupException& ex)
+ {
+ fprintf(stderr, "Discarding pose: Transform reference lookup exception\n");
+ success = false;
+ }
+ catch(libTF::TransformReference::ExtrapolateException& ex)
+ {
+ fprintf(stderr, "Discarding pose: Extrapolation exception: %s\n", ex.what());
+ success = false;
+ }
+ catch(...)
+ {
+ fprintf(stderr, "Discarding pose: Exception in pose computation\n");
+ success = false;
+ }
+
+ if (success)
+ {
+ m_basePos[0] = pose.x;
+ m_basePos[1] = pose.y;
+ m_basePos[2] = pose.yaw;
+
+ baseUpdate();
+ }
+ }
+
+ virtual void baseUpdate(void)
+ {
+ }
+
+ rosTFClient m_tf;
+ std_msgs::RobotBase2DOdom m_localizedPose;
+ robot_desc::URDF *m_urdf;
+ planning_models::KinematicModel *m_kmodel;
+ double m_basePos[3];
+
+ };
+
+}
Added: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h (rev 0)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-08-13 22:37:32 UTC (rev 3013)
@@ -0,0 +1,166 @@
+/*********************************************************************
+* 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 <ros/node.h>
+#include <ros/time.h>
+#include <urdf/URDF.h>
+#include <planning_models/kinematic.h>
+#include <std_msgs/RobotBase2DOdom.h>
+#include <rosTF/rosTF.h>
+
+namespace planning_node_util
+{
+
+ class NodeWithRobotModel : public ros::node
+ {
+
+ public:
+
+ NodeWithRobotModel(const std::string &robot_model, const std::string &name, uint32_t options = 0) : ros::node(name, options),
+ m_tf(*this, true, 1 * 1000000000ULL, 1000000000ULL)
+ {
+ m_urdf = NULL;
+ m_kmodel = NULL;
+ m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
+ loadRobotDescription(robot_model);
+
+ subscribe("localizedpose", m_localizedPose, &NodeWithRobotModel::localizedPoseCallback);
+ }
+
+ virtual ~NodeWithRobotModel(void)
+ {
+ if (m_urdf)
+ delete m_urdf;
+ if (m_kmodel)
+ delete m_kmodel;
+ }
+
+ void setRobotDescriptionFromData(const char *data)
+ {
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadString(data))
+ setRobotDescription(file);
+ else
+ delete file;
+ }
+
+ void setRobotDescriptionFromFile(const char *filename)
+ {
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadFile(filename))
+ setRobotDescription(file);
+ else
+ delete file;
+ }
+
+ virtual void setRobotDescription(robot_desc::URDF *file)
+ {
+ if (m_urdf)
+ delete m_urdf;
+ if (m_kmodel)
+ delete m_kmodel;
+
+ m_urdf = file;
+ m_kmodel = new planning_models::KinematicModel();
+ m_kmodel->setVerbose(false);
+ m_kmodel->build(*file);
+ }
+
+ virtual void loadRobotDescription(const std::string &robot_model)
+ {
+ if (!robot_model.empty() && robot_model != "-")
+ {
+ std::string content;
+ if (get_param(robot_model, content))
+ setRobotDescriptionFromData(content.c_str());
+ else
+ fprintf(stderr, "Robot model '%s' not found!\n", robot_model.c_str());
+ }
+ }
+
+ protected:
+
+ void localizedPoseCallback(void)
+ {
+ bool success = true;
+ libTF::TFPose2D pose;
+ pose.x = m_localizedPose.pos.x;
+ pose.y = m_localizedPose.pos.y;
+ pose.yaw = m_localizedPose.pos.th;
+ pose.time = m_localizedPose.header.stamp.to_ull();
+ pose.frame = m_localizedPose.header.frame_id;
+
+ try
+ {
+ pose = m_tf.transformPose2D("FRAMEID_MAP", pose);
+ }
+ catch(libTF::TransformReference::LookupException& ex)
+ {
+ fprintf(stderr, "Discarding pose: Transform reference lookup exception\n");
+ success = false;
+ }
+ catch(libTF::TransformReference::ExtrapolateException& ex)
+ {
+ fprintf(stderr, "Discarding pose: Extrapolation exception: %s\n", ex.what());
+ success = false;
+ }
+ catch(...)
+ {
+ fprintf(stderr, "Discarding pose: Exception in pose computation\n");
+ success = false;
+ }
+
+ if (success)
+ {
+ m_basePos[0] = pose.x;
+ m_basePos[1] = pose.y;
+ m_basePos[2] = pose.yaw;
+
+ baseUpdate();
+ }
+ }
+
+ virtual void baseUpdate(void)
+ {
+ }
+
+ rosTFClient m_tf;
+ std_msgs::RobotBase2DOdom m_localizedPose;
+ robot_desc::URDF *m_urdf;
+ planning_models::KinematicModel *m_kmodel;
+ double m_basePos[3];
+
+ };
+
+}
Copied: pkg/trunk/motion_planning/planning_node_util/manifest.xml (from rev 3011, pkg/trunk/motion_planning/collision_space/manifest.xml)
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/manifest.xml (rev 0)
+++ pkg/trunk/motion_planning/planning_node_util/manifest.xml 2008-08-13 22:37:32 UTC (rev 3013)
@@ -0,0 +1,21 @@
+<package>
+ <description brief="A set of node classes">
+ A set of classes derived from ros::node that allow access to robot
+ models and/or collision spaces.
+ </description>
+
+ <author>Ioan Sucan/is...@wi...</author>
+ <license>BSD</license>
+
+ <depend package="roscpp"/>
+ <depend package="std_msgs"/>
+ <depend package="rosTF"/>
+ <depend package="wg_robot_description_parser"/>
+ <depend package="planning_models"/>
+ <depend package="collision_space"/>
+
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags=""/>
+ </export>
+
+</package>
Modified: pkg/trunk/world_models/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-13 21:31:23 UTC (rev 3012)
+++ pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-13 22:37:32 UTC (rev 3013)
@@ -11,4 +11,5 @@
<depend package="wg_robot_description_parser" />
<depend package="planning_models" />
<depend package="collision_space" />
+<depend package="planning_node_util" />
</package>
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-13 21:31:23 UTC (rev 3012)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-13 22:37:32 UTC (rev 3013)
@@ -96,7 +96,7 @@
- @b "world_3d_map/verbosity_level" : @b [int] sets the verbosity level (default 1)
**/
-#include <planning_models/node.h>
+#include <planning_node_util/knode.h>
#include <rosthread/member_thread.h>
#include <rosthread/mutex.h>
@@ -111,11 +111,11 @@
#include <deque>
#include <cmath>
-class World3DMap : public planning_models::NodeWithRobotModel
+class World3DMap : public planning_node_util::NodeWithRobotModel
{
public:
- World3DMap(const std::string &robot_model) : planning_models::NodeWithRobotModel(robot_model, "world_3d_map")
+ World3DMap(const std::string &robot_model) : planning_node_util::NodeWithRobotModel(robot_model, "world_3d_map")
{
advertise<std_msgs::PointCloudFloat32>("world_3d_map");
advertise<rostools::Log>("roserr");
@@ -158,7 +158,7 @@
virtual void setRobotDescription(robot_desc::URDF *file)
{
- planning_models::NodeWithRobotModel::setRobotDescription(file);
+ planning_node_util::NodeWithRobotModel::setRobotDescription(file);
addSelfSeeBodies();
}
@@ -172,7 +172,7 @@
void baseUpdate(void)
{
- planning_models::NodeWithRobotModel::baseUpdate();
+ planning_node_util::NodeWithRobotModel::baseUpdate();
int group = m_kmodel->getGroupID(m_urdf->getRobotName() + "::base");
if (group >= 0)
m_kmodel->computeTransforms(m_basePos, group);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
