Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16854] pkg/trunk
|
From: <is...@us...> - 2009-06-09 05:44:53
|
Revision: 16854
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16854&view=rev
Author: isucan
Date: 2009-06-09 05:43:35 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
documentation + better output handling
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/mainpage.dox
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/world_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODESimple.h
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/include/planning_models/output.h
pkg/trunk/motion_planning/planning_models/src/output.cpp
pkg/trunk/world_models/collision_space/src/bodies.cpp
pkg/trunk/world_models/collision_space/src/environment.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/collision_space/src/environmentODESimple.cpp
Removed Paths:
-------------
pkg/trunk/world_models/collision_space/include/collision_space/output.h
pkg/trunk/world_models/collision_space/src/collision_space/
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -43,7 +43,7 @@
namespace planning_environment
{
- /** A class capable of loading a robot model from the parameter server */
+ /** \brief A class capable of loading a robot model from the parameter server */
class CollisionModels : public RobotModels
{
@@ -58,7 +58,7 @@
{
}
- /** Reload the robot description and recreate the model */
+ /** \brief Reload the robot description and recreate the model */
virtual void reload(void)
{
RobotModels::reload();
@@ -66,7 +66,7 @@
loadCollision();
}
- /** Return the instance of the constructed ODE collision model */
+ /** \brief Return the instance of the constructed ODE collision model */
const boost::shared_ptr<collision_space::EnvironmentModel> &getODECollisionModel(void) const
{
return ode_collision_model_;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -78,37 +78,45 @@
return collisionSpace_;
}
+ /** \brief Return the instance of collision models that is being used */
CollisionModels* getCollisionModels(void) const
{
return cm_;
}
- /** Define a callback for before updating a map */
+ /** \brief Define a callback for before updating a map */
void setOnBeforeMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
{
onBeforeMapUpdate_ = callback;
}
- /** Define a callback for after updating a map */
+ /** \brief Define a callback for after updating a map */
void setOnAfterMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
{
onAfterMapUpdate_ = callback;
}
- /** Define a callback for after updating a map */
+ /** \brief Define a callback for after updating a map */
void setOnAfterAttachBodyCallback(const boost::function<void(planning_models::KinematicModel::Link*)> &callback)
{
onAfterAttachBody_ = callback;
}
+ /** \brief Return true if map has been received */
bool haveMap(void) const
{
return haveMap_;
}
- /** Return true if a map update has been received in the last sec seconds */
+ /** \brief Return true if a map update has been received in the last sec seconds */
bool isMapUpdated(double sec) const;
+ /** \brief Return the last update time for the map */
+ const ros::Time& lastMapUpdate(void) const
+ {
+ return lastMapUpdate_;
+ }
+
protected:
void setupCSM(void);
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -77,71 +77,73 @@
delete robotState_;
}
- /** Define a callback for when the state is changed */
+ /** \brief Define a callback for when the state is changed */
void setOnStateUpdateCallback(const boost::function<void(void)> &callback)
{
onStateUpdate_ = callback;
}
+ /** \brief Get the kinematic model that is being monitored */
planning_models::KinematicModel* getKinematicModel(void) const
{
return kmodel_;
}
-
+
+ /** \brief Get the instance of @b RobotModels that is being used */
RobotModels* getRobotModels(void) const
{
return rm_;
}
- /** Return a pointer to the maintained robot state */
+ /** \brief Return a pointer to the maintained robot state */
const planning_models::KinematicModel::StateParams* getRobotState(void) const
{
return robotState_;
}
- /** Return the frame id of the state */
+ /** \brief Return the frame id of the state */
const std::string& getFrameId(void) const
{
return frame_id_;
}
- /** Return true if a pose has been received */
+ /** \brief Return true if a pose has been received */
bool havePose(void) const
{
return havePose_;
}
- /** Return true if a full mechanism state has been received */
+ /** \brief Return true if a full mechanism state has been received */
bool haveState(void) const
{
return haveMechanismState_;
}
- /** Return the time of the last pose update */
+ /** \brief Return the time of the last pose update */
const ros::Time& lastPoseUpdate(void) const
{
return lastPoseUpdate_;
}
- /** Return the time of the last state update */
+ /** \brief Return the time of the last state update */
const ros::Time& lastStateUpdate(void) const
{
return lastStateUpdate_;
}
- /** Wait until a pose is received */
+ /** \brief Wait until a pose is received */
void waitForPose(void) const;
- /** Wait until a full mechanism state is received */
+ /** \brief Wait until a full mechanism state is received */
void waitForState(void) const;
- /** Return true if a pose has been received in the last sec seconds */
+ /** \brief Return true if a pose has been received in the last sec seconds */
bool isPoseUpdated(double sec) const;
- /** Return true if a full mechanism state has been received in the last sec seconds */
+ /** \brief Return true if a full mechanism state has been received in the last sec seconds */
bool isStateUpdated(double sec) const;
- /** Output the current state as ROS INFO */
+ /** \brief Output the current state as ROS INFO */
void printRobotState(void) const;
protected:
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -49,13 +49,13 @@
namespace planning_environment
{
- /** A class capable of loading a robot model from the parameter server */
+ /** \brief A class capable of loading a robot model from the parameter server */
class RobotModels
{
public:
- /** A class to define a planner configuration */
+ /** \brief A class to define a planner configuration */
class PlannerConfig
{
public:
@@ -89,54 +89,67 @@
{
}
+ /** \brief Return the name of the description */
const std::string &getDescription(void) const
{
return description_;
}
- /** Return the instance of the constructed kinematic model */
+ /** \brief Return the instance of the constructed kinematic model */
const boost::shared_ptr<planning_models::KinematicModel> &getKinematicModel(void) const
{
return kmodel_;
}
- /** Return the instance of the parsed robot description */
+ /** \brief Return the instance of the parsed robot description */
const boost::shared_ptr<robot_desc::URDF> &getParsedDescription(void) const
{
return urdf_;
}
+ /** \brief Return the map of the planning groups (arrays of link names) */
const std::map< std::string, std::vector<std::string> > &getPlanningGroups(void) const
{
return planning_groups_;
}
+ /** \brief Return the names of the links that should be considered when performing collision checking */
const std::vector<std::string> &getCollisionCheckLinks(void) const
{
return collision_check_links_;
}
-
+
+ /** \brief Return the names of the links that should be considered when cleaning sensor data
+ of parts the robot can see from itself */
const std::vector<std::string> &getSelfSeeLinks(void) const
{
return self_see_links_;
}
+ /** \brief Return the groups of links that should be considered when testing for self collision. This is an
+ array of pairs. Both elements of the pair are groups of links. If any link in the first member of the pair
+ collides with some link in the second member of the pair, we have a collision */
const std::vector< std::pair < std::vector<std::string>, std::vector<std::string> > > &getSelfCollisionGroups(void) const
{
return self_collision_check_groups_;
}
+ /** \brief Return true if models have been loaded */
bool loadedModels(void) const
{
return loaded_models_;
}
+ /** \brief Get the amount of padding to be used for links when cleaning sensor data */
double getSelfSeePadding(void);
+
+ /** \brief Get the amount of scaling to be used for links when cleaning sensor data */
double getSelfSeeScale(void);
+ /** \breif Get the list of planner configurations available for a specific planning group */
std::vector< boost::shared_ptr<PlannerConfig> > getGroupPlannersConfig(const std::string &group);
- /** Reload the robot description and recreate the model */
+ /** \brief Reload the robot description and recreate the model */
virtual void reload(void);
protected:
Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-06-09 05:43:35 UTC (rev 16854)
@@ -2,7 +2,10 @@
\mainpage
\htmlinclude manifest.html
-\b planning_environment is ...
+\b planning_environment is a library that allows users to instantiate
+robot models and collision models based on data from the parameter
+server with minimal user input. Additionally, state information for
+both robot models and collision environments can be monitored.
<!--
In addition to providing an overview of your package,
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -54,7 +54,13 @@
collisionSpace_ = cm_->getODECollisionModel().get();
collisionMapSubscriber_ = nh_.subscribe("collision_map", 1, &CollisionSpaceMonitor::collisionMapCallback, this);
- attachBodySubscriber_ = nh_.subscribe("attach_object", 1, &CollisionSpaceMonitor::attachObjectCallback, this);
+ ROS_DEBUG("Listening to collision_map");
+
+ if (cm_->loadedModels())
+ {
+ attachBodySubscriber_ = nh_.subscribe("attach_object", 1, &CollisionSpaceMonitor::attachObjectCallback, this);
+ ROS_DEBUG("Listening to attach_object");
+ }
}
bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
@@ -94,7 +100,7 @@
double tupd = (ros::WallTime::now() - startTime).toSec();
ROS_DEBUG("Updated map model in %f seconds", tupd);
- lastMapUpdate_ = ros::Time::now();
+ lastMapUpdate_ = collisionMap->header.stamp;
haveMap_ = true;
if (onAfterMapUpdate_ != NULL)
@@ -139,7 +145,7 @@
// update the collision model
collisionSpace_->updateAttachedBodies();
- ROS_INFO("Link '%s' on '%s' has %d objects attached", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str(), n);
+ ROS_DEBUG("Link '%s' on '%s' has %d objects attached", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str(), n);
}
else
ROS_WARN("Unable to attach object to link '%s' on '%s'", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str());
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -60,11 +60,18 @@
floatingJoint_ = kmodel_->getRobot(0)->chain->name;
if (includePose_)
+ {
localizedPoseSubscriber_ = nh_.subscribe("localized_pose", 1, &KinematicModelStateMonitor::localizedPoseCallback, this);
+ ROS_DEBUG("Listening to localized pose");
+ }
else
+ {
frame_id_ = kmodel_->getRobot(0)->chain->after->name;
+ ROS_DEBUG("Robot state frame is %s", frame_id_.c_str());
+ }
}
mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
+ ROS_DEBUG("Listening to mechanism state");
}
}
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -35,9 +35,54 @@
/** \author Ioan Sucan */
#include "planning_environment/robot_models.h"
+#include <planning_models/output.h>
#include <ros/console.h>
#include <sstream>
+// make sure messages from planning_environments & collision_space go to ROS console
+namespace planning_environment
+{
+ class OutputHandlerROScon : public planning_models::msg::OutputHandler
+ {
+ public:
+
+ OutputHandlerROScon(void) : OutputHandler()
+ {
+ planning_models::msg::useOutputHandler(this);
+ }
+
+ ~OutputHandlerROScon(void)
+ {
+ planning_models::msg::noOutputHandler();
+ }
+
+ /** Issue a ROS error */
+ virtual void error(const std::string &text)
+ {
+ ROS_ERROR("%s", text.c_str());
+ }
+
+ /** Issue a ROS warning */
+ virtual void warn(const std::string &text)
+ {
+ ROS_WARN("%s", text.c_str());
+ }
+
+ /** Issue ROS info */
+ virtual void inform(const std::string &text)
+ {
+ ROS_INFO("%s", text.c_str());
+ }
+
+ /** Issue ROS debug */
+ virtual void message(const std::string &text)
+ {
+ ROS_DEBUG("%s", text.c_str());
+ }
+ };
+ static OutputHandlerROScon _outputHandler;
+}
+
void planning_environment::RobotModels::reload(void)
{
kmodel_.reset();
@@ -74,7 +119,10 @@
getSelfSeeLinks(self_see_links_);
}
else
+ {
urdf_.reset();
+ ROS_ERROR("Unable to parse URDF description!");
+ }
}
else
ROS_ERROR("Robot model '%s' not found!", description_.c_str());
Modified: pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml 2009-06-09 05:43:35 UTC (rev 16854)
@@ -1,7 +1,7 @@
<launch>
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
- <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find planning_environment)/planning.yaml" />
+ <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find pr2_defs)/planning/planning.yaml" />
<test test-name="test_robot_models" pkg="planning_environment" type="test_robot_models" />
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-09 05:43:35 UTC (rev 16854)
@@ -6,7 +6,8 @@
set(ROS_BUILD_TYPE Debug)
rospack_add_library(planning_models src/kinematic.cpp
- src/load_mesh.cpp)
+ src/load_mesh.cpp
+ src/output.cpp)
# Unit tests
rospack_add_gtest(test_kinematic test/test_kinematic.cpp)
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -38,7 +38,7 @@
#define PLANNING_MODELS_KINEMATIC_ROBOT_MODEL_
#include "planning_models/shapes.h"
-
+#include "planning_models/output.h"
#include <urdf/URDF.h>
#include <LinearMath/btTransform.h>
#include <boost/thread/mutex.hpp>
@@ -49,12 +49,8 @@
#include <map>
#include <cassert>
-/** @htmlinclude ../../manifest.html
+/** Describing a kinematic robot model loaded from URDF. Visual geometry is ignored */
- @mainpage
-
- A class describing a kinematic robot model loaded from URDF. Visual geometry is ignored */
-
/** Main namespace */
namespace planning_models
{
@@ -551,6 +547,7 @@
protected:
KinematicModel *m_owner;
+ msg::Interface m_msg;
ModelInfo &m_mi;
double *m_params;
std::map<unsigned int, bool> m_seen;
@@ -653,7 +650,8 @@
bool m_built;
boost::mutex m_lock;
-
+ msg::Interface m_msg;
+
private:
/** Build the needed datastructure for a joint */
Added: pkg/trunk/motion_planning/planning_models/include/planning_models/output.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/output.h (rev 0)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/output.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -0,0 +1,133 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/* \author Ioan Sucan */
+
+#ifndef PLANNING_MODELS_OUTPUT_INTERFACE_
+#define PLANNING_MODELS_OUTPUT_INTERFACE_
+
+#include <string>
+#include <cstdarg>
+
+/** Main namespace */
+namespace planning_models
+{
+
+ /** Message namespace */
+ namespace msg
+ {
+
+ /** The piece of code that desires interaction with an action
+ or an output handler should use an instance of this
+ class */
+ class Interface
+ {
+ public:
+
+ Interface(void);
+ virtual ~Interface(void);
+
+ void inform(const std::string &text) const;
+ void warn(const std::string &text) const;
+ void error(const std::string &text) const;
+ void message(const std::string &text) const;
+
+ void inform(const char *msg, ...) const;
+ void warn(const char *msg, ...) const;
+ void error(const char *msg, ...) const;
+ void message(const char *msg, ...) const;
+
+ protected:
+
+ std::string combine(const char *msg, va_list ap) const;
+
+ };
+
+ /** Generic class to handle output from a piece of code */
+ class OutputHandler
+ {
+ public:
+
+ OutputHandler(void)
+ {
+ }
+
+ virtual ~OutputHandler(void)
+ {
+ }
+
+ /** Print an error message: "Error: ...." */
+ virtual void error(const std::string &text) = 0;
+
+ /** Print an warning message: "Warning: ...." */
+ virtual void warn(const std::string &text) = 0;
+
+ /** Print some information: "Information: ...." */
+ virtual void inform(const std::string &text) = 0;
+
+ /** Print a simple message */
+ virtual void message(const std::string &text) = 0;
+ };
+
+ class OutputHandlerSTD : public OutputHandler
+ {
+ public:
+
+ OutputHandlerSTD(void) : OutputHandler()
+ {
+ }
+
+ /** Print an error message: "Error: ...." */
+ virtual void error(const std::string &text);
+
+ /** Print an warning message: "Warning: ...." */
+ virtual void warn(const std::string &text);
+
+ /** Print some information: "Information: ...." */
+ virtual void inform(const std::string &text);
+
+ /** Print a simple message */
+ virtual void message(const std::string &text);
+
+
+ };
+
+ void noOutputHandler(void);
+ void useOutputHandler(OutputHandler *oh);
+ OutputHandler* getOutputHandler(void);
+ }
+
+}
+
+#endif
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -36,6 +36,7 @@
#include <planning_models/kinematic.h>
#include <algorithm>
+#include <sstream>
#include <cmath>
namespace planning_models
@@ -378,7 +379,7 @@
{
if (m_built)
{
- std::cerr << "Model has already been built!" << std::endl;
+ m_msg.error("Model has already been built!");
return;
}
@@ -555,11 +556,11 @@
if (m_verbose && joint->usedParams > 0)
{
- std::cout << "Joint '" << urdfLink->joint->name << "' connects link '" << urdfLink->parentName << "' to link '" <<
- urdfLink->name << "' and uses state coordinates: ";
+ m_msg.message("Joint '" + urdfLink->joint->name + "' connects link '" + urdfLink->parentName + "' to link '" + urdfLink->name + "' and uses state coordinates: ");
+ std::stringstream ss;
for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
- std::cout << i + robot->stateDimension << " ";
- std::cout << std::endl;
+ ss << i + robot->stateDimension << " ";
+ m_msg.message(ss.str());
}
robot->stateDimension += joint->usedParams;
@@ -588,6 +589,7 @@
planning_models::KinematicModel::Joint* planning_models::KinematicModel::createJoint(const robot_desc::URDF::Link* urdfLink)
{
Joint *newJoint = NULL;
+ std::stringstream ss;
switch (urdfLink->joint->type)
{
case robot_desc::URDF::Link::Joint::FIXED:
@@ -606,7 +608,8 @@
newJoint = new RevoluteJoint();
break;
default:
- std::cerr << "Unknown joint type " << urdfLink->joint->type << std::endl;
+ ss << urdfLink->joint->type;
+ m_msg.error("Unknown joint type " + ss.str());
break;
}
return newJoint;
@@ -744,7 +747,7 @@
}
}
else
- std::cerr << "Unknown joint: '" << name << "'" << std::endl;
+ m_msg.error("Unknown joint: '" + name + "'");
return result;
}
@@ -878,10 +881,12 @@
for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
if (m_mi.groupStateIndexList[groupID][i] == pos)
return i;
- std::cerr << "Joint '" << name << "' is not in group " << groupID << std::endl;
+ std::stringstream ss;
+ ss << groupID;
+ m_msg.error("Joint '" + name + "' is not in group " + ss.str());
}
}
- std::cerr << "Unknown joint: '" << name << "'" << std::endl;
+ m_msg.error("Unknown joint: '" + name + "'");
return -1;
}
@@ -899,7 +904,7 @@
return;
}
}
- std::cerr << "Unknown joint: '" << name << "'" << std::endl;
+ m_msg.error("Unknown joint: '" + name + "'");
}
void planning_models::KinematicModel::StateParams::copyParams(double *params) const
Added: pkg/trunk/motion_planning/planning_models/src/output.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/output.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_models/src/output.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -0,0 +1,176 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/* \author Ioan Sucan */
+
+#include "planning_models/output.h"
+#include <boost/thread/mutex.hpp>
+#include <iostream>
+#include <cstdlib>
+
+static planning_models::msg::OutputHandlerSTD _defaultOutputHandler;
+static planning_models::msg::OutputHandler *OUTPUT_HANDLER = static_cast<planning_models::msg::OutputHandler*>(&_defaultOutputHandler);
+static boost::mutex _lock; // it is likely the outputhandler does some I/O, so we serialize it
+
+void planning_models::msg::noOutputHandler(void)
+{
+ _lock.lock();
+ OUTPUT_HANDLER = NULL;
+ _lock.unlock();
+}
+
+void planning_models::msg::useOutputHandler(OutputHandler *oh)
+{
+ _lock.lock();
+ OUTPUT_HANDLER = oh;
+ _lock.unlock();
+}
+
+planning_models::msg::OutputHandler* planning_models::msg::getOutputHandler(void)
+{
+ return OUTPUT_HANDLER;
+}
+
+planning_models::msg::Interface::Interface(void)
+{
+}
+
+planning_models::msg::Interface::~Interface(void)
+{
+}
+
+void planning_models::msg::Interface::message(const std::string &text) const
+{
+ if (OUTPUT_HANDLER)
+ {
+ _lock.lock();
+ OUTPUT_HANDLER->message(text);
+ _lock.unlock();
+ }
+}
+
+void planning_models::msg::Interface::message(const char *msg, ...) const
+{
+ va_list ap;
+ va_start(ap, msg);
+ message(combine(msg, ap));
+ va_end(ap);
+}
+
+void planning_models::msg::Interface::inform(const std::string &text) const
+{
+ if (OUTPUT_HANDLER)
+ {
+ _lock.lock();
+ OUTPUT_HANDLER->inform(text);
+ _lock.unlock();
+ }
+}
+
+void planning_models::msg::Interface::inform(const char *msg, ...) const
+{
+ va_list ap;
+ va_start(ap, msg);
+ inform(combine(msg, ap));
+ va_end(ap);
+}
+
+void planning_models::msg::Interface::warn(const std::string &text) const
+{
+ if (OUTPUT_HANDLER)
+ {
+ _lock.lock();
+ OUTPUT_HANDLER->warn(text);
+ _lock.unlock();
+ }
+}
+
+void planning_models::msg::Interface::warn(const char *msg, ...) const
+{
+ va_list ap;
+ va_start(ap, msg);
+ warn(combine(msg, ap));
+ va_end(ap);
+}
+
+void planning_models::msg::Interface::error(const std::string &text) const
+{
+ if (OUTPUT_HANDLER)
+ {
+ _lock.lock();
+ OUTPUT_HANDLER->error(text);
+ _lock.unlock();
+ }
+}
+
+void planning_models::msg::Interface::error(const char *msg, ...) const
+{
+ va_list ap;
+ va_start(ap, msg);
+ error(combine(msg, ap));
+ va_end(ap);
+}
+std::string planning_models::msg::Interface::combine(const char *msg, va_list va) const
+{
+ va_list ap;
+ va_copy(ap, va);
+ char buf[1024];
+ vsnprintf(buf, sizeof(buf), msg, ap);
+ va_end(ap);
+ return buf;
+}
+
+void planning_models::msg::OutputHandlerSTD::error(const std::string &text)
+{
+ std::cerr << "Error: " << text << std::endl;
+ std::cerr.flush();
+}
+
+void planning_models::msg::OutputHandlerSTD::warn(const std::string &text)
+{
+ std::cerr << "Warning: " << text << std::endl;
+ std::cerr.flush();
+}
+
+void planning_models::msg::OutputHandlerSTD::inform(const std::string &text)
+{
+ std::cout << "Info: " << text << std::endl;
+ std::cout.flush();
+}
+
+void planning_models::msg::OutputHandlerSTD::message(const std::string &text)
+{
+ std::cout << text;
+ std::cout.flush();
+}
Modified: pkg/trunk/world_models/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-06-09 05:43:35 UTC (rev 16854)
@@ -6,11 +6,10 @@
set(ROS_BUILD_TYPE Debug)
-rospack_add_library(collision_space src/collision_space/output.cpp
- src/collision_space/bodies.cpp
- src/collision_space/environment.cpp
- src/collision_space/environmentODESimple.cpp
- src/collision_space/environmentODE.cpp)
+rospack_add_library(collision_space src/bodies.cpp
+ src/environment.cpp
+ src/environmentODESimple.cpp
+ src/environmentODE.cpp)
rospack_link_boost(collision_space thread)
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -37,18 +37,19 @@
#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_
#define COLLISION_SPACE_ENVIRONMENT_MODEL_
-#include "collision_space/output.h"
#include <planning_models/kinematic.h>
+#include <planning_models/output.h>
#include <LinearMath/btVector3.h>
#include <boost/thread/mutex.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>
#include <string>
-/** @htmlinclude ../../manifest.html
- @mainpage
-
+/** Main namespace */
+namespace collision_space
+{
+ /**
A class describing an environment for a kinematic robot. This is
the base (abstract) definition. Different implementations are
possible. The class is aware of a certain set of fixed
@@ -56,13 +57,7 @@
can change (removed by clearObstacles()) and a kinematic
robot model. The class provides functionality for checking whether a
given robot is in collision.
-
- */
-
-/** Main namespace */
-namespace collision_space
-{
-
+ */
class EnvironmentModel
{
public:
@@ -168,7 +163,7 @@
boost::mutex m_lock;
bool m_selfCollision;
bool m_verbose;
- msg::Interface m_msg;
+ planning_models::msg::Interface m_msg;
/** List of loaded robot models */
boost::shared_ptr<planning_models::KinematicModel> m_robotModel;
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -40,13 +40,10 @@
#include "collision_space/environment.h"
#include <ode/ode.h>
-/** @htmlinclude ../../manifest.html
-
- A class describing an environment for a kinematic robot using ODE */
-
namespace collision_space
{
+ /** A class describing an environment for a kinematic robot using ODE */
class EnvironmentModelODE : public EnvironmentModel
{
public:
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODESimple.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODESimple.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODESimple.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -40,13 +40,11 @@
#include "collision_space/environment.h"
#include <ode/ode.h>
-/** @htmlinclude ../../manifest.html
- A class describing an environment for a kinematic robot using ODE */
-
namespace collision_space
{
-
+
+ /** A class describing an environment for a kinematic robot using ODE */
class EnvironmentModelODESimple : public EnvironmentModel
{
public:
Deleted: pkg/trunk/world_models/collision_space/include/collision_space/output.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/output.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/world_models/collision_space/include/collision_space/output.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -1,133 +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.
-*********************************************************************/
-
-/* \author Ioan Sucan */
-
-#ifndef COLLISION_SPACE_OUTPUT_INTERFACE_
-#define COLLISION_SPACE_OUTPUT_INTERFACE_
-
-#include <string>
-#include <cstdarg>
-
-/** Main namespace */
-namespace collision_space
-{
-
- /** Message namespace */
- namespace msg
- {
-
- /** The piece of code that desires interaction with an action
- or an output handler should use an instance of this
- class */
- class Interface
- {
- public:
-
- Interface(void);
- virtual ~Interface(void);
-
- void inform(const std::string &text) const;
- void warn(const std::string &text) const;
- void error(const std::string &text) const;
- void message(const std::string &text) const;
-
- void inform(const char *msg, ...) const;
- void warn(const char *msg, ...) const;
- void error(const char *msg, ...) const;
- void message(const char *msg, ...) const;
-
- protected:
-
- std::string combine(const char *msg, va_list ap) const;
-
- };
-
- /** Generic class to handle output from a piece of code */
- class OutputHandler
- {
- public:
-
- OutputHandler(void)
- {
- }
-
- virtual ~OutputHandler(void)
- {
- }
-
- /** Print an error message: "Error: ...." */
- virtual void error(const std::string &text) = 0;
-
- /** Print an warning message: "Warning: ...." */
- virtual void warn(const std::string &text) = 0;
-
- /** Print some information: "Information: ...." */
- virtual void inform(const std::string &text) = 0;
-
- /** Print a simple message */
- virtual void message(const std::string &text) = 0;
- };
-
- class OutputHandlerSTD : public OutputHandler
- {
- public:
-
- OutputHandlerSTD(void) : OutputHandler()
- {
- }
-
- /** Print an error message: "Error: ...." */
- virtual void error(const std::string &text);
-
- /** Print an warning message: "Warning: ...." */
- virtual void warn(const std::string &text);
-
- /** Print some information: "Information: ...." */
- virtual void inform(const std::string &text);
-
- /** Print a simple message */
- virtual void message(const std::string &text);
-
-
- };
-
- void noOutputHandler(void);
- void useOutputHandler(OutputHandler *oh);
- OutputHandler* getOutputHandler(void);
- }
-
-}
-
-#endif
Copied: pkg/trunk/world_models/collision_space/src/bodies.cpp (from rev 16825, pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp)
===================================================================
--- pkg/trunk/world_models/collision_space/src/bodies.cpp (rev 0)
+++ pkg/trunk/world_models/collision_space/src/bodies.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -0,0 +1,383 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include "collision_space/bodies.h"
+#include <planning_models/output.h>
+#include <LinearMath/btConvexHull.h>
+#include <cmath>
+
+
+static planning_models::msg::Interface MSG;
+
+collision_space::bodies::Body* collision_space::bodies::createBodyFromShape(const planning_models::shapes::Shape *shape)
+{
+ Body *body = NULL;
+
+ switch (shape->type)
+ {
+ case planning_models::shapes::Shape::BOX:
+ body = new collision_space::bodies::Box(shape);
+ break;
+ case planning_models::shapes::Shape::SPHERE:
+ body = new collision_space::bodies::Sphere(shape);
+ break;
+ case planning_models::shapes::Shape::CYLINDER:
+ body = new collision_space::bodies::Cylinder(shape);
+ break;
+ case planning_models::shapes::Shape::MESH:
+ body = new collision_space::bodies::ConvexMesh(shape);
+ break;
+ default:
+ MSG.error("Creating body from shape: Unknown shape type %d", shape->type);
+ break;
+ }
+
+ return body;
+}
+
+void collision_space::bodies::mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere)
+{
+ if (spheres.empty())
+ {
+ mergedSphere.center.setValue(btScalar(0), btScalar(0), btScalar(0));
+ mergedSphere.radius = 0.0;
+ }
+ else
+ {
+ mergedSphere = spheres[0];
+ for (unsigned int i = 1 ; i < spheres.size() ; ++i)
+ {
+ if (spheres[i].radius <= 0.0)
+ continue;
+ double d = spheres[i].center.distance(mergedSphere.center);
+ if (d + mergedSphere.radius <= spheres[i].radius)
+ {
+ mergedSphere.center = spheres[i].center;
+ mergedSphere.radius = spheres[i].radius;
+ }
+ else
+ if (d + spheres[i].radius > mergedSphere.radius)
+ {
+ btVector3 delta = mergedSphere.center - spheres[i].center;
+ mergedSphere.radius = (delta.length() + spheres[i].radius + mergedSphere.radius)/2.0;
+ mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center;
+ }
+ }
+ }
+}
+
+bool collision_space::bodies::Sphere::containsPoint(const btVector3 &p) const
+{
+ return (m_center - p).length2() < m_radius2;
+}
+
+void collision_space::bodies::Sphere::useDimensions(const planning_models::shapes::Shape *shape) // radius
+{
+ m_radius = static_cast<const planning_models::shapes::Sphere*>(shape)->radius;
+}
+
+void collision_space::bodies::Sphere::updateInternalData(void)
+{
+ m_radiusU = m_radius * m_scale + m_padding;
+ m_radius2 = m_radiusU * m_radiusU;
+ m_center = m_pose.getOrigin();
+}
+
+double collision_space::bodies::Sphere::computeVolume(void) const
+{
+ return 4.0 * M_PI * m_radiusU * m_radiusU * m_radiusU / 3.0;
+}
+
+void collision_space::bodies::Sphere::computeBoundingSphere(BoundingSphere &sphere) const
+{
+ sphere.center = m_center;
+ sphere.radius = m_radiusU;
+}
+
+bool collision_space::bodies::Cylinder::containsPoint(const btVector3 &p) const
+{
+ btVector3 v = p - m_center;
+ double pH = v.dot(m_normalH);
+
+ if (fabs(pH) > m_length2)
+ return false;
+
+ double pB1 = v.dot(m_normalB1);
+ double remaining = m_radius2 - pB1 * pB1;
+
+ if (remaining < 0.0)
+ return false;
+ else
+ {
+ double pB2 = v.dot(m_normalB2);
+ return pB2 * pB2 < remaining;
+ }
+}
+
+void collision_space::bodies::Cylinder::useDimensions(const planning_models::shapes::Shape *shape) // (length, radius)
+{
+ m_length = static_cast<const planning_models::shapes::Cylinder*>(shape)->length;
+ m_radius = static_cast<const planning_models::shapes::Cylinder*>(shape)->radius;
+}
+
+void collision_space::bodies::Cylinder::updateInternalData(void)
+{
+ m_radiusU = m_radius * m_scale + m_padding;
+ m_radius2 = m_radiusU * m_radiusU;
+ m_length2 = m_scale * m_length / 2.0 + m_padding;
+ m_center = m_pose.getOrigin();
+ m_radiusB = std::max(m_radiusU, m_length2);
+
+ const btMatrix3x3& basis = m_pose.getBasis();
+ m_normalB1 = basis.getColumn(0);
+ m_normalB2 = basis.getColumn(1);
+ m_normalH = basis.getColumn(2);
+}
+
+double collision_space::bodies::Cylinder::computeVolume(void) const
+{
+ return 2.0 * M_PI * m_radiusU * m_radiusU * m_length2;
+}
+
+void collision_space::bodies::Cylinder::computeBoundingSphere(BoundingSphere &sphere) const
+{
+ sphere.center = m_center;
+ sphere.radius = m_radiusB;
+}
+
+bool collision_space::bodies::Box::containsPoint(const btVector3 &p) const
+{
+ btVector3 v = p - m_center;
+ double pL = v.dot(m_normalL);
+
+ if (fabs(pL) > m_length2)
+ return false;
+
+ double pW = v.dot(m_normalW);
+
+ if (fabs(pW) > m_width2)
+ return false;
+
+ double pH = v.dot(m_normalH);
+
+ if (fabs(pH) > m_height2)
+ return false;
+
+ return true;
+}
+
+void collision_space::bodies::Box::useDimensions(const planning_models::shapes::Shape *shape) // (x, y, z) = (length, width, height)
+{
+ const double *size = static_cast<const planning_models::shapes::Box*>(shape)->size;
+ m_length = size[0];
+ m_width = size[1];
+ m_height = size[2];
+}
+
+void collision_space::bodies::Box::updateInternalData(void)
+{
+ double s2 = m_scale / 2.0;
+ m_length2 = m_length * s2 + m_padding;
+ m_width2 = m_width * s2 + m_padding;
+ m_height2 = m_height * s2 + m_padding;
+
+ m_center = m_pose.getOrigin();
+
+ m_radiusB = sqrt(m_length2 * m_length2 + m_width2 * m_width2 + m_height2 * m_height2);
+
+ const btMatrix3x3& basis = m_pose.getBasis();
+ m_normalL = basis.getColumn(0);
+ m_normalW = basis.getColumn(1);
+ m_normalH = basis.getColumn(2);
+}
+
+double collision_space::bodies::Box::computeVolume(void) const
+{
+ return 8.0 * m_length2 * m_width2 * m_height2;
+}
+
+void collision_space::bodies::Box::computeBoundingSphere(BoundingSphere &sphere) const
+{
+ sphere.center = m_center;
+ sphere.radius = m_radiusB;
+}
+
+bool collision_space::bodies::ConvexMesh::containsPoint(const btVector3 &p) const
+{
+ btVector3 ip = (m_iPose * p) / m_scale;
+ return isPointInsidePlanes(ip);
+}
+
+void collision_space::bodies::ConvexMesh::useDimensions(const planning_models::shapes::Shape *shape)
+{
+ const planning_models::shapes::Mesh *mesh = static_cast<const planning_models::shapes::Mesh*>(shape);
+ m_planes.clear();
+ m_triangles.clear();
+ m_vertices.clear();
+ m_radiusB = 0.0;
+ m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
+
+ btVector3 *vertices = new btVector3[mesh->vertexCount];
+ for(unsigned int i = 0; i < mesh->vertexCount ; ++i)
+ {
+ vertices[i].setX(mesh->vertices[3 * i ]);
+ vertices[i].setY(mesh->vertices[3 * i + 1]);
+ vertices[i].setZ(mesh->vertices[3 * i + 2]);
+ }
+
+ HullDesc hd(QF_TRIANGLES, mesh->vertexCount, vertices);
+ HullResult hr;
+ HullLibrary hl;
+ if (hl.CreateConvexHull(hd, hr) == QE_OK)
+ {
+ MSG.inform("Convex hull has %d(%d) vertices (down from %d), %d faces", hr.m_OutputVertices.size(), hr.mNumOutputVertices, mesh->vertexCount, hr.mNumFaces);
+
+ m_vertices.reserve(hr.m_OutputVertices.size());
+ btVector3 sum(btScalar(0), btScalar(0), btScalar(0));
+ for (int j = 0 ; j < hr.m_OutputVertices.size() ; ++j)
+ {
+ m_vertices.push_back(hr.m_OutputVertices[j]);
+ sum = sum + hr.m_OutputVertices[j];
+ }
+
+ m_meshCenter = sum / (double)(hr.m_OutputVertices.size());
+ for (unsigned int j = 0 ; j < m_vertices.size() ; ++j)
+ {
+ double dist = m_vertices[j].distance2(m_meshCenter);
+ if (dist > m_radiusB)
+ m_radiusB = dist;
+ }
+ m_radiusB = sqrt(m_radiusB) * m_scale + m_padding;
+
+ m_triangles.reserve(hr.m_Indices.size());
+ for (int j = 0 ; j < hr.m_Indices.size() ; ++j)
+ m_triangles.push_back(hr.m_Indices[j]);
+
+ for (unsigned int j = 0 ; j < hr.mNumFaces ; ++j)
+ {
+ const btVector3 &p1 = hr.m_OutputVertices[hr.m_Indices[j * 3 ]];
+ const btVector3 &p2 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]];
+ const btVector3 &p3 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]];
+
+ btVector3 edge1 = (p2 - p1);
+ btVector3 edge2 = (p3 - p1);
+
+ edge1.normalize();
+ edge2.normalize();
+
+ btVector3 planeNormal = edge1.cross(edge2);
+
+ if (planeNormal.length2() > btScalar(1e-6))
+ {
+ planeNormal.normalize();
+ btVector4 planeEquation(planeNormal.getX(), planeNormal.getY(), planeNormal.getZ(), -planeNormal.dot(p1));
+
+ unsigned int behindPlane = countVerticesBehindPlane(planeEquation);
+ if (behindPlane > 0)
+ {
+ btVector4 planeEquation2(-planeEquation.getX(), -planeEquation.getY(), -planeEquation.getZ(), -planeEquation.getW());
+ unsigned int behindPlane2 = countVerticesBehindPlane(planeEquation2);
+ if (behindPlane2 < behindPlane)
+ {
+ planeEquation.setValue(planeEquation2.getX(), planeEquation2.getY(), planeEquation2.getZ(), planeEquation2.getW());
+ behindPlane = behindPlane2;
+ }
+ }
+
+ if (behindPlane > 0)
+ MSG.warn("Approximate plane: %u of %u points are behind the plane", behindPlane, (unsigned int)m_vertices.size());
+
+ m_planes.push_back(planeEquation);
+ }
+ }
+ }
+ else
+ MSG.error("Unable to compute convex hull.");
+
+ hl.ReleaseResult(hr);
+ delete[] vertices;
+}
+
+void collision_space::bodies::ConvexMesh::updateInternalData(void)
+{
+ m_iPose = m_pose.inverse();
+ m_center = m_pose.getOrigin() + m_meshCenter;
+}
+
+void collision_space::bodies::ConvexMesh::computeBoundingSphere(BoundingSphere &sphere) const
+{
+ sphere.center = m_center;
+ sphere.radius = m_radiusB;
+}
+
+bool collision_space::bodies::ConvexMesh::isPointInsidePlanes(const btVector3& point) const
+{
+ unsigned int numplanes = m_planes.size();
+ for (unsigned int i = 0 ; i < numplanes ; ++i)
+ {
+ const btVector4& plane = m_planes[i];
+ btScalar dist = btScalar(plane.dot(point)) + plane.getW() - btScalar(m_padding) - btScalar(1e-6);
+ if (dist > btScalar(0))
+ return false;
+ }
+ return true;
+}
+
+unsigned int collision_space::bodies::ConvexMesh::countVerticesBehindPlane(const btVector4& planeNormal) const
+{
+ unsigned int numvertices = m_vertices.size();
+ unsigned int result = 0;
+ for (unsigned int i = 0 ; i < numvertices ; ++i)
+ {
+ btScalar dist = btScalar(planeNormal.dot(m_vertices[i])) + btScalar(planeNormal.getW()) - btScalar(1e-6);
+ if (dist > btScalar(0))
+ result++;
+ }
+ return result;
+}
+
+double collision_space::bodies::ConvexMesh::computeVolume(void) const
+{
+ double volume = 0.0;
+ for (unsigned int i = 0 ; i < m_triangles.size() / 3 ; ++i)
+ {
+ const btVector3 &v1 = m_vertices[m_triangles[3*i + 0]];
+ const btVector3 &v2 = m_vertices[m_triangles[3*i + 1]];
+ const btVector3 &v3 = m_vertices[m_triangles[3*i + 2]];
+ volume += v1.x()*v2.y()*v3.z() + v2.x()*v3.y()*v1.z() + v3.x()*v1.y()*v2.z() -v1.x()*v3.y()*v2.z() - v2.x()*v1.y()*v3.z() - v3.x()*v2.y()*v1.z();
+ }
+ return fabs(volume)/6.0;
+}
Copied: pkg/trunk/world_models/collision_space/src/environment.cpp (from rev 16825, pkg/trunk/world_models/collision_space/src/collision_space/environment.cpp)
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment.cpp (rev 0)
+++ pkg/trunk/world_models/collision_space/src/environment.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -0,0 +1,67 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include "collision_space/environment.h"
+
+void collision_space::EnvironmentModel::setVerbose(bool verbose)
+{
+ m_verbose = verbose;
+}
+
+void collision_space::EnvironmentModel::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+{
+ m_robotModel = model;
+}
+
+void collision_space::EnvironmentModel::lock(void)
+{
+ m_lock.lock();
+}
+
+void collision_space::EnvironmentModel::unlock(void)
+{
+ m_lock.unlock();
+}
+
+void collision_space::EnvironmentModel::setSelfCollision(bool selfCollision)
+{
+ m_selfCollision = selfCollision;
+}
+
+bool collision_space::EnvironmentModel::getSelfCollision(void) const
+{
+ return m_selfCollision;
+}
Property changes on: pkg/trunk/world_models/collision_space/src/environment.cpp
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/world_models/collision_space/src/collision_space/environment.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/world_models/collision_space/src/environmentODE.cpp (from rev 16825, pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp)
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp (rev 0)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -0,0 +1,577 @@
+/*********************************************************************
+* 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 DAMAG...
[truncated message content] |
