Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/hpp-fcl
  • coal-library/coal
2 results
Show changes
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 3218 deletions
branches/point_cloud/CMakeLists.txt deleted 100644 → 0
View file @ c45c79a7
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
# directories (or patterns, but directories should suffice) that should
# be excluded from the distro. This is not the place to put things that
# should be ignored everywhere, like "build" directories; that happens in
# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
# ready for inclusion in a distro.
#
# This list is combined with the list in rosbuild/rosbuild.cmake. Note
# that CMake 2.6 may be required to ensure that the two lists are combined
# properly. CMake 2.4 seems to have unpredictable scoping rules for such
# variables.
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
rosbuild_make_distribution(0.1.0)
branches/point_cloud/Makefile deleted 100644 → 0
View file @ c45c79a7
include $(shell rospack find mk)/cmake_stack.mk
\ No newline at end of file
branches/point_cloud/ann/Makefile deleted 100644 → 0
View file @ c45c79a7
all: installed
#
# Download, extract and compile from a released tarball:
#
TARBALL = build/ann_1.1.2.tar.gz
TARBALL_URL = http://www.cs.umd.edu/~mount/ANN/Files/1.1.2/ann_1.1.2.tar.gz
TARBALL_PATCH = fANN.diff
INITIAL_DIR = build/ann_1.1.2
SOURCE_DIR = build/ann-tar
include $(shell rospack find mk)/download_unpack_build.mk
INSTALL_DIR = ann
CMAKE = cmake
CMAKE_ARGS = -D CMAKE_BUILD_TYPE="Release" -D CMAKE_INSTALL_PREFIX=`rospack find ann`/$(INSTALL_DIR)
MAKE_ARGS = linux-g++
MAKE = make
installed: wiped $(SOURCE_DIR)/unpacked
cd $(SOURCE_DIR) && make $(ROS_PARALLEL_JOBS) $(MAKE_ARGS)
mkdir -p $(INSTALL_DIR)/include
mkdir -p $(INSTALL_DIR)/include/ann
cp -r $(SOURCE_DIR)/include/ANN/*.h $(INSTALL_DIR)/include/ann
cp -r $(SOURCE_DIR)/bin $(INSTALL_DIR)
cp -r $(SOURCE_DIR)/lib $(INSTALL_DIR)
touch installed
clean:
rm -rf build
rm -rf $(INSTALL_DIR) installed
wiped: Makefile
make wipe
touch wiped
wipe: clean
rm -rf build patched
branches/point_cloud/ann/fANN.diff deleted 100644 → 0
View file @ c45c79a7
diff -purN ann_1.1.2/include/ANN/ANN.h ann_1.1.2_modified/include/ANN/ANN.h
--- include/ANN/ANN.h 2010-01-27 20:40:01.000000000 -0800
+++ include/ANN/ANN.h 2011-08-30 12:13:26.629632052 -0700
@@ -232,7 +232,7 @@ const ANNdist ANN_DIST_INF = ANN_DBL_MAX
// strictly positive.
//----------------------------------------------------------------------
-const ANNbool ANN_ALLOW_SELF_MATCH = ANNtrue;
+const ANNbool ANN_ALLOW_SELF_MATCH = ANNfalse;
//----------------------------------------------------------------------
// Norms and metrics:
diff -purN ann_1.1.2/Make-config ann_1.1.2_modified/Make-config
--- Make-config 2010-01-27 20:40:01.000000000 -0800
+++ Make-config 2011-08-30 12:12:42.916876291 -0700
@@ -72,7 +72,7 @@ linux-g++:
$(MAKE) targets \
"ANNLIB = libANN.a" \
"C++ = g++" \
- "CFLAGS = -O3" \
+ "CFLAGS = -O3 -fPIC" \
"MAKELIB = ar ruv" \
"RANLIB = true"
branches/point_cloud/ann/fPIC.diff deleted 100644 → 0
View file @ c45c79a7
--- Make-config 2010-01-27 20:40:01.000000000 -0800
+++ Make-config 2011-08-25 14:29:00.000000000 -0700
@@ -72,7 +72,7 @@ linux-g++:
$(MAKE) targets \
"ANNLIB = libANN.a" \
"C++ = g++" \
- "CFLAGS = -O3" \
+ "CFLAGS = -O3 -fPIC" \
"MAKELIB = ar ruv" \
"RANLIB = true"
branches/point_cloud/ann/mainpage.dox deleted 100644 → 0
View file @ c45c79a7
/**
\mainpage
\htmlinclude manifest.html
\b ann is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/
branches/point_cloud/ann/manifest.xml deleted 100644 → 0
View file @ c45c79a7
<package>
<description brief="ann">
This package is a wrapper on the ann library available from <a href="http://www.cs.umd.edu/~mount/ANN/">ANN library</a>. This package does not modify the contents of the original library in any manner and only wraps it for easy distribution with the ROS packaging system.
</description>
<author>Maintained by Jia Pan, Sachin Chitta</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/ann</url>
<export>
<cpp cflags="-I${prefix}/ann/include" lflags="-L${prefix}/ann/lib -Wl,-rpath,${prefix}/ann/lib -lANN"/>
</export>
</package>
branches/point_cloud/collision_space_fcl/CMakeLists.txt deleted 100644 → 0
View file @ c45c79a7
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
rosbuild_add_library(collision_space_fcl
src/environment.cpp
src/environment_objects.cpp
src/environmentFCL.cpp)
rosbuild_add_gtest(test_collision_space_fcl test/test_collision_space_fcl.cpp)
target_link_libraries(test_collision_space_fcl collision_space_fcl)
target_link_libraries(test_collision_space_fcl assimp)
\ No newline at end of file
branches/point_cloud/collision_space_fcl/Makefile deleted 100644 → 0
View file @ c45c79a7
include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environment.h deleted 100644 → 0
View file @ c45c79a7
/*********************************************************************
* 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_FCL_ENVIRONMENT_MODEL_H
#define COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_H
#include "collision_space_fcl/environment_objects.h"
#include <planning_models/kinematic_model.h>
#include <planning_models/kinematic_state.h>
#include <geometric_shapes/bodies.h>
#include <LinearMath/btVector3.h>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>
#include <string>
/** \brief Main namespace */
namespace collision_space_fcl
{
/** \brief 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 set of
obstacles and a robot model. The obstacles are placed in different
namespaces so they can be added and removed selectively.
*/
class EnvironmentModel
{
public:
enum BodyType {
LINK,
ATTACHED,
OBJECT
};
/** \brief Definition of a contact point */
struct Contact
{
/** \brief contact position */
btVector3 pos;
/** \brief normal unit vector at contact */
btVector3 normal;
/** \brief depth (penetration between bodies) */
double depth;
/** \brief The first body involved in the contact */
std::string body_name_1;
BodyType body_type_1;
/** \brief The first body involved in the contact */
std::string body_name_2;
BodyType body_type_2;
};
/** \brief Definition of a contact that is allowed */
struct AllowedContact
{
/// the bound where the contact is allowed
boost::shared_ptr<bodies::Body> bound;
std::string body_name_1;
std::string body_name_2;
/// tha maximum depth for the contact
double depth;
};
typedef std::map<std::string, std::map<std::string, std::vector<AllowedContact> > > AllowedContactMap;
/** \brief Definition of a structure for the allowed collision matrix */
/* False means that no collisions are allowed, true means ok */
class AllowedCollisionMatrix
{
public:
AllowedCollisionMatrix(){
valid_ = true;
}
AllowedCollisionMatrix(const std::vector<std::string>& names,
bool allowed = false);
AllowedCollisionMatrix(const std::vector<std::vector<bool> >& all_coll_vectors,
const std::map<std::string, unsigned int>& all_coll_indices);
AllowedCollisionMatrix(const AllowedCollisionMatrix& acm);
bool getAllowedCollision(const std::string& name1, const std::string& name2,
bool& allowed_collision) const;
bool getAllowedCollision(unsigned int i, unsigned int j,
bool& allowed_collision) const;
bool hasEntry(const std::string& name) const;
bool getEntryIndex(const std::string& name,
unsigned int& index) const;
bool getEntryName(const unsigned int ind,
std::string& name) const;
bool removeEntry(const std::string& name);
bool addEntry(const std::string& name, bool allowed);
bool changeEntry(bool allowed);
bool changeEntry(const std::string& name, bool allowed);
bool changeEntry(const std::string& name1,
const std::string& name2,
bool allowed);
bool changeEntry(const unsigned int ind_1,
const unsigned int ind_2,
bool allowed);
bool changeEntry(const std::string& name,
const std::vector<std::string>& change_names,
bool allowed);
bool changeEntry(const std::vector<std::string>& change_names_1,
const std::vector<std::string>& change_names_2,
bool allowed);
bool getValid() const {
return valid_;
};
unsigned int getSize() const {
return allowed_entries_.size();
}
typedef boost::bimap<std::string, unsigned int> entry_type;
const entry_type& getEntriesBimap() const {
return allowed_entries_bimap_;
}
void print(std::ostream& out) const;
private:
bool valid_;
std::vector<std::vector<bool> > allowed_entries_;
entry_type allowed_entries_bimap_;
};
EnvironmentModel(void)
{
verbose_ = false;
objects_ = new EnvironmentObjects();
use_altered_collision_matrix_ = false;
}
virtual ~EnvironmentModel(void)
{
if (objects_)
delete objects_;
}
/**********************************************************************/
/* Collision Environment Configuration */
/**********************************************************************/
/** \brief Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
used to increase or decrease the size of the robot's
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
virtual void setRobotModel(const planning_models::KinematicModel* model,
const AllowedCollisionMatrix& acm,
const std::map<std::string, double>& link_padding_map,
double default_padding = 0.0,
double scale = 1.0);
/** \brief Get robot scale */
double getRobotScale(void) const;
/** \brief Get robot padding */
double getRobotPadding(void) const;
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(const planning_models::KinematicState* state) = 0;
/** \brief Update the set of bodies that are attached to the robot (re-creates them) */
virtual void updateAttachedBodies() = 0;
/** \brief Get the robot model */
const planning_models::KinematicModel* getRobotModel(void) const;
/**********************************************************************/
/* Collision Checking Routines */
/**********************************************************************/
/** \brief Check if a model is in collision with environment and self. Contacts are not computed */
virtual bool isCollision(void) const = 0;
/** \brief Check for self collision. Contacts are not computed */
virtual bool isSelfCollision(void) const = 0;
/** \brief Check whether the model is in collision with the environment. Self collisions are not checked */
virtual bool isEnvironmentCollision(void) const = 0;
/** \brief Get the list of contacts (collisions). The maximum total number of contacts to be returned can be specified, and the max per pair of objects that are in collision*/
virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const = 0;
/** \brief This function will get the complete list of contacts between any two potentially colliding bodies. The num per contacts specifies the number of contacts per pair that will be returned */
virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const = 0;
/**********************************************************************/
/* Collision Bodies */
/**********************************************************************/
/** \brief Remove all objects from collision model */
virtual void clearObjects(void) = 0;
/** \brief Remove objects from a specific namespace in the collision model */
virtual void clearObjects(const std::string &ns) = 0;
/** \brief Tells whether or not there is an object with the given name in the collision model */
virtual bool hasObject(const std::string& ns) const = 0;
/** \brief Add a static collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
virtual void addObject(const std::string &ns, shapes::StaticShape *shape) = 0;
/** \brief Add a collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment.*/
virtual void addObject(const std::string &ns, shapes::Shape* shape, const btTransform &pose) = 0;
/** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. Memory allocated for the shapes is freed by the collision environment.*/
virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses) = 0;
virtual void getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const = 0;
/** \briefs Sets a temporary robot padding on the indicated links */
virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map);
/** \briefs Reverts link padding to that set at robot initialization */
virtual void revertAlteredLinkPadding();
const std::map<std::string,double>& getDefaultLinkPaddingMap() const;
std::map<std::string,double> getCurrentLinkPaddingMap() const;
double getCurrentLinkPadding(std::string name) const;
/** \brief Get the objects currently contained in the model */
const EnvironmentObjects* getObjects(void) const;
const AllowedCollisionMatrix& getDefaultAllowedCollisionMatrix() const;
const AllowedCollisionMatrix& getCurrentAllowedCollisionMatrix() const;
/** \brief set the matrix for collision touch to use in lieu of the default settings */
virtual void setAlteredCollisionMatrix(const AllowedCollisionMatrix& acm);
/** \brief reverts to using default settings for allowed collisions */
virtual void revertAlteredCollisionMatrix();
/** \brief sets the allowed contacts that will be used in collision checking */
virtual void setAllowedContacts(const std::vector<AllowedContact>& allowed_contacts);
/** \brief gets the allowed contacts that will be used in collision checking */
const std::vector<AllowedContact>& getAllowedContacts() const;
/** \brief clear out all allowed contacts */
void clearAllowedContacts() {
allowed_contact_map_.clear();
allowed_contacts_.clear();
}
/**********************************************************************/
/* Miscellaneous Routines */
/**********************************************************************/
/** \brief Provide interface to a lock. Use carefully! */
void lock(void) const;
/** \brief Provide interface to a lock. Use carefully! */
void unlock(void) const;
/** \brief Enable/disable verbosity */
void setVerbose(bool verbose);
/** \brief Check the state of verbosity */
bool getVerbose(void) const;
/** \brief Clone the environment. */
virtual EnvironmentModel* clone(void) const = 0;
protected:
/** \brief Mutex used to lock the datastructure */
mutable boost::recursive_mutex lock_;
/** \brief Flag to indicate whether verbose mode is on */
bool verbose_;
/** \brief Loaded robot model */
const planning_models::KinematicModel* robot_model_;
/** \brief List of objects contained in the environment */
EnvironmentObjects *objects_;
/** \brief Scaling used for robot links */
double robot_scale_;
/** \brief padding used for robot links */
double default_robot_padding_;
AllowedCollisionMatrix default_collision_matrix_;
AllowedCollisionMatrix altered_collision_matrix_;
bool use_altered_collision_matrix_;
std::map<std::string, double> default_link_padding_map_;
std::map<std::string, double> altered_link_padding_map_;
bool use_altered_link_padding_map_;
AllowedContactMap allowed_contact_map_;
std::vector<AllowedContact> allowed_contacts_;
};
}
#endif
branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environmentFCL.h deleted 100644 → 0
View file @ c45c79a7
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, 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 Willow Garage, Inc. 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 Jia Pan */
#ifndef COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_FCL_H
#define COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_FCL_H
#include "collision_space_fcl/environment.h"
#include "fcl/broad_phase_collision.h"
#include <map>
#include <vector>
namespace collision_space_fcl
{
/** \brief A class describing an environment for a kinematic robot using ODE */
class EnvironmentModelFCL : public EnvironmentModel
{
friend bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata);
public:
EnvironmentModelFCL(void);
virtual ~EnvironmentModelFCL(void);
/** \brief Get the list of contacts (collisions). The maximum total number of contacts to be returned can be specified, and the max per pair of objects that's in collision*/
virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const;
/** \brief This function will get the complete list of contacts between any two potentially colliding bodies. The num per contacts specifies the number of contacts per pair that will be returned */
virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const;
/** \brief Check if a model is in collision */
virtual bool isCollision(void) const;
/** \brief Check if a model is in self collision */
virtual bool isSelfCollision(void) const;
/** \brief Check if a model is in environment collision */
virtual bool isEnvironmentCollision(void) const;
/** \brief Remove all objects from collision model */
virtual void clearObjects(void);
/** \brief Remove objects from a specific namespace in the collision model */
virtual void clearObjects(const std::string& ns);
/** \brief Tells whether or not there is an object with the given name in the collision model */
virtual bool hasObject(const std::string& ns) const;
/** \brief Add a static collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
virtual void addObject(const std::string& ns, shapes::StaticShape* shape);
/** \brief Add a collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
virtual void addObject(const std::string& ns, shapes::Shape* shape, const btTransform& pose);
/** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. Memory allocated for the shapes is freed by the collision environment. */
virtual void addObjects(const std::string& ns, const std::vector<shapes::Shape*>& shapes, const std::vector<btTransform>& poses);
virtual void getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const;
/** \brief Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
used to increase or decrease the size of the robot's
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
virtual void setRobotModel(const planning_models::KinematicModel* model,
const AllowedCollisionMatrix& allowed_collision_matrix,
const std::map<std::string, double>& link_padding_map,
double default_padding = 0.0,
double scale = 1.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(const planning_models::KinematicState* state);
/** \brief Update the set of bodies that are attached to the robot (re-creates them) */
virtual void updateAttachedBodies(void);
/** \brief Update the set of bodies that are attached to the robot (re-creates them) using the indicated padding or default if non-specified */
virtual void updateAttachedBodies(const std::map<std::string, double>& link_padding_map);
/** \briefs Sets a temporary robot padding on the indicated links */
virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map);
/** \briefs Reverts link padding to that set at robot initialization */
virtual void revertAlteredLinkPadding();
/** \brief Clone the environment */
virtual EnvironmentModel* clone(void) const;
protected:
/** \brief Geometry for attachment */
struct AttGeom
{
~AttGeom()
{
for(unsigned int i = 0; i < geom.size(); i++)
{
delete geom[i];
}
for(unsigned int i = 0; i < padded_geom.size(); i++)
{
delete padded_geom[i];
}
}
std::vector<fcl::CollisionObject*> geom; // managed by self_geom_manager
std::vector<fcl::CollisionObject*> padded_geom; // for collision with environment geometry
const planning_models::KinematicModel::AttachedBodyModel* att;
unsigned int index;
};
/** \brief Geometry for Link */
struct LinkGeom
{
~LinkGeom()
{
for(unsigned int i = 0; i < geom.size(); i++)
{
delete geom[i];
}
for(unsigned int i = 0; i < padded_geom.size(); i++)
{
delete padded_geom[i];
}
deleteAttachedBodies();
}
void deleteAttachedBodies()
{
for(unsigned int i = 0; i < att_bodies.size(); i++)
{
delete att_bodies[i];
}
att_bodies.clear();
}
std::vector<fcl::CollisionObject*> geom; // managed by self_geom_manager
std::vector<fcl::CollisionObject*> padded_geom; // for collision with environment geometry
std::vector<AttGeom*> att_bodies;
const planning_models::KinematicModel::LinkModel* link;
unsigned int index;
};
struct CollisionData
{
CollisionData(void)
{
done = false;
collides = false;
max_contacts_total = 0;
max_contacts_pair = 0;
contacts = NULL;
allowed_collision_matrix = NULL;
allowed = NULL;
exhaustive = false;
}
//these are parameters
unsigned int max_contacts_total;
unsigned int max_contacts_pair;
const AllowedCollisionMatrix* allowed_collision_matrix;
const std::map<fcl::CollisionObject*, std::pair<std::string, BodyType> >* geom_lookup_map;
const AllowedContactMap *allowed;
bool exhaustive;
//these are for return info
bool done;
bool collides;
std::vector<EnvironmentModelFCL::Contact>* contacts;
//for the last collision found
std::string body_name_1;
BodyType body_type_1;
std::string body_name_2;
BodyType body_type_2;
};
struct ModelInfo
{
std::vector<LinkGeom*> link_geom;
};
struct CollisionNamespace
{
CollisionNamespace(const std::string& nm) : name(nm)
{
env_geom_manager.reset(new fcl::SSaPCollisionManager());
}
virtual ~CollisionNamespace(void)
{
clear();
}
void clear(void)
{
for(unsigned int i = 0; i < geoms.size(); ++i)
delete geoms[i];
geoms.clear();
env_geom_manager->clear();
}
std::string name;
std::vector<fcl::CollisionObject*> geoms;
boost::shared_ptr<fcl::BroadPhaseCollisionManager> env_geom_manager;
};
/** \brief Internal function for collision detection */
void testCollision(CollisionData* data) const;
/** \brief Internal function for collision detection */
void testSelfCollision(CollisionData* data) const;
/** \brief Internal function for collision detection */
void testEnvironmentCollision(CollisionData* data) const;
/** \brief Internal function for collision detection */
void testObjectCollision(CollisionNamespace* cn, CollisionData* data) const;
fcl::CollisionObject* copyGeom(fcl::CollisionObject* geom) const;
void createRobotModel();
fcl::CollisionObject* createGeom(const shapes::Shape* shape, double scale, double padding);
fcl::CollisionObject* createGeom(const shapes::StaticShape* shape);
void updateGeom(fcl::CollisionObject* geom, const btTransform& pose) const;
void addAttachedBody(LinkGeom* lg, const planning_models::KinematicModel::AttachedBodyModel* attm,
double padd);
std::map<std::string, bool> attached_bodies_in_collision_matrix_;
void setAttachedBodiesLinkPadding();
void revertAttachedBodiesLinkPadding();
void freeMemory(void);
ModelInfo model_geom_;
std::map<std::string, CollisionNamespace*> coll_namespaces_;
std::map<fcl::CollisionObject*, std::pair<std::string, BodyType> > geom_lookup_map_;
bool previous_set_robot_model_;
boost::shared_ptr<fcl::BroadPhaseCollisionManager> self_geom_manager;
};
}
#endif
branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environment_objects.h deleted 100644 → 0
View file @ c45c79a7
/*********************************************************************
* 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, E. Gil Jones */
#ifndef COLLISION_SPACE_FCL_ENVIRONMENT_OBJECTS_H
#define COLLISION_SPACE_FCL_ENVIRONMENT_OBJECTS_H
#include <vector>
#include <string>
#include <map>
#include <geometric_shapes/shapes.h>
#include <LinearMath/btTransform.h>
namespace collision_space_fcl
{
/** \brief List of objects contained in the environment (not including robot links) */
class EnvironmentObjects
{
public:
EnvironmentObjects(void)
{
}
~EnvironmentObjects(void)
{
clearObjects();
}
struct NamespaceObjects
{
/** \brief An array of static shapes */
std::vector< shapes::StaticShape* > static_shape;
/** \brief An array of shapes */
std::vector< shapes::Shape* > shape;
/** \brief An array of shape poses */
std::vector< btTransform > shape_pose;
};
/** \brief Get the list of namespaces */
std::vector<std::string> getNamespaces(void) const;
/** \brief Get the list of objects */
const NamespaceObjects& getObjects(const std::string &ns) const;
/** \brief Get the list of objects */
NamespaceObjects& getObjects(const std::string &ns);
/** \brief Add a static object to the namespace. The user releases ownership of the object. */
void addObject(const std::string &ns, shapes::StaticShape *shape);
/** \brief Add an object to the namespace. The user releases ownership of the object. */
void addObject(const std::string &ns, shapes::Shape *shape, const btTransform &pose);
/** \brief Remove object. Object equality is verified by comparing pointers. Ownership of the object is renounced upon. Returns true on success. */
bool removeObject(const std::string &ns, const shapes::Shape *shape);
/** \brief Remove object. Object equality is verified by comparing pointers. Ownership of the object is renounced upon. Returns true on success. */
bool removeObject(const std::string &ns, const shapes::StaticShape *shape);
/** \brief Clear the objects in a specific namespace. Memory is freed. */
void clearObjects(const std::string &ns);
/** \brief Clear all objects. Memory is freed. */
void clearObjects(void);
/** \brief Adds namespace without necessary adding a shape. */
void addObjectNamespace(const std::string ns);
/** \brief Clone this instance of the class */
EnvironmentObjects* clone(void) const;
private:
std::map<std::string, NamespaceObjects> objects_;
NamespaceObjects empty_;
};
}
#endif
branches/point_cloud/collision_space_fcl/mainpage.dox deleted 100644 → 0
View file @ c45c79a7
/**
\mainpage
\htmlinclude manifest.html
\b collision_space_fcl is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/
branches/point_cloud/collision_space_fcl/manifest.xml deleted 100644 → 0
View file @ c45c79a7
<package>
<description brief="collision_space_fcl">
collision_space_fcl
</description>
<author>Jia Pan</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="rosconsole"/>
<depend package="planning_models"/>
<depend package="geometric_shapes"/>
<depend package="bullet"/>
<depend package="fcl"/>
<url>http://ros.org/wiki/collision_space_fcl</url>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollision_space_fcl `rosboost-cfg --lflags thread`"/>
</export>
</package>
branches/point_cloud/collision_space_fcl/src/environment.cpp deleted 100644 → 0
View file @ c45c79a7
/*********************************************************************
* 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_fcl/environment.h"
#include <ros/console.h>
#include <iomanip>
collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names,
bool allowed)
{
unsigned int ns = names.size();
allowed_entries_.resize(ns);
for(unsigned int i = 0; i < ns; i++) {
allowed_entries_[i].resize(ns,allowed);
allowed_entries_bimap_.insert(entry_type::value_type(names[i], i));
}
valid_ = true;
}
collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::vector<bool> >& all_coll_vectors,
const std::map<std::string, unsigned int>& all_coll_indices)
{
unsigned int num_outer = all_coll_vectors.size();
valid_ = true;
if(all_coll_indices.size() != all_coll_vectors.size()) {
valid_ = false;
ROS_WARN_STREAM("Indices size " << all_coll_indices.size() << " not equal to num vecs " << all_coll_vectors.size());
return;
}
for(std::map<std::string, unsigned int>::const_iterator it = all_coll_indices.begin();
it != all_coll_indices.end();
it++) {
allowed_entries_bimap_.insert(entry_type::value_type(it->first, it->second));
}
if(allowed_entries_bimap_.left.size() != all_coll_indices.size()) {
valid_ = false;
ROS_WARN_STREAM("Some strings or values in allowed collision matrix are repeated");
}
if(allowed_entries_bimap_.right.begin()->first != 0) {
valid_ = false;
ROS_WARN_STREAM("No entry with index 0 in map");
}
if(allowed_entries_bimap_.right.rbegin()->first != num_outer-1) {
valid_ = false;
ROS_WARN_STREAM("Last index should be " << num_outer << " but instead is " << allowed_entries_bimap_.right.rbegin()->first);
}
for(unsigned int i = 0; i < num_outer; i++) {
if(num_outer != all_coll_vectors[i].size()) {
valid_ = false;
ROS_WARN_STREAM("Entries size for " << allowed_entries_bimap_.right.at(i) << " is " << all_coll_vectors[i].size() << " instead of " << num_outer);
}
}
allowed_entries_ = all_coll_vectors;
}
collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const AllowedCollisionMatrix& acm)
{
valid_ = acm.valid_;
allowed_entries_ = acm.allowed_entries_;
allowed_entries_bimap_ = acm.allowed_entries_bimap_;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getAllowedCollision(const std::string& name1,
const std::string& name2,
bool& allowed_collision) const
{
entry_type::left_const_iterator it1 = allowed_entries_bimap_.left.find(name1);
if(it1 == allowed_entries_bimap_.left.end()) {
return false;
}
entry_type::left_const_iterator it2 = allowed_entries_bimap_.left.find(name2);
if(it2 == allowed_entries_bimap_.left.end()) {
return false;
}
if(it1->second > allowed_entries_.size()) {
ROS_INFO_STREAM("Something wrong with acm entry for " << name1);
return false;
}
if(it2->second > allowed_entries_[it1->second].size()) {
ROS_INFO_STREAM("Something wrong with acm entry for " << name2);
return false;
}
allowed_collision = allowed_entries_[it1->second][it2->second];
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getAllowedCollision(unsigned int i, unsigned int j,
bool& allowed_collision) const
{
if(i > allowed_entries_.size() || j > allowed_entries_[i].size()) {
return false;
}
allowed_collision = allowed_entries_[i][j];
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::hasEntry(const std::string& name) const
{
return(allowed_entries_bimap_.left.find(name) != allowed_entries_bimap_.left.end());
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getEntryIndex(const std::string& name,
unsigned int& index) const
{
entry_type::left_const_iterator it1 = allowed_entries_bimap_.left.find(name);
if(it1 == allowed_entries_bimap_.left.end()) {
return false;
}
index = it1->second;
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getEntryName(const unsigned int ind,
std::string& name) const
{
entry_type::right_const_iterator it1 = allowed_entries_bimap_.right.find(ind);
if(it1 == allowed_entries_bimap_.right.end()) {
return false;
}
name = it1->second;
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::removeEntry(const std::string& name) {
if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end()) {
return false;
}
unsigned int last_index = allowed_entries_bimap_.size()-1;
unsigned int ind = allowed_entries_bimap_.left.find(name)->second;
allowed_entries_.erase(allowed_entries_.begin()+ind);
for(unsigned int i = 0; i < allowed_entries_.size(); i++) {
allowed_entries_[i].erase(allowed_entries_[i].begin()+ind);
}
allowed_entries_bimap_.left.erase(name);
//if this is last ind, no need to decrement
if(ind != last_index) {
//sanity checks
entry_type::right_iterator it = allowed_entries_bimap_.right.find(last_index);
if(it == allowed_entries_bimap_.right.end()) {
ROS_INFO_STREAM("Something wrong with last index " << last_index << " ind " << ind);
}
//now we need to decrement the index for everything after this
for(unsigned int i = ind+1; i <= last_index; i++) {
entry_type::right_iterator it = allowed_entries_bimap_.right.find(i);
if(it == allowed_entries_bimap_.right.end()) {
ROS_WARN_STREAM("Problem in replace " << i);
return false;
}
bool successful_replace = allowed_entries_bimap_.right.replace_key(it, i-1);
if(!successful_replace) {
ROS_WARN_STREAM("Can't replace");
return false;
}
}
}
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::addEntry(const std::string& name,
bool allowed)
{
if(allowed_entries_bimap_.left.find(name) != allowed_entries_bimap_.left.end()) {
return false;
}
unsigned int ind = allowed_entries_.size();
allowed_entries_bimap_.insert(entry_type::value_type(name,ind));
std::vector<bool> new_entry(ind+1, allowed);
allowed_entries_.resize(ind+1);
allowed_entries_[ind] = new_entry;
for(unsigned int i = 0; i < ind; i++) {
allowed_entries_[i].resize(ind+1);
allowed_entries_[i][ind] = allowed;
}
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(bool allowed)
{
for(unsigned int i = 0; i < allowed_entries_.size(); i++) {
for(unsigned int j = 0; j < allowed_entries_[i].size(); j++) {
allowed_entries_[i][j] = allowed;
allowed_entries_[j][i] = allowed;
}
}
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name1,
const std::string& name2,
bool allowed) {
entry_type::left_const_iterator it1 = allowed_entries_bimap_.left.find(name1);
if(it1 == allowed_entries_bimap_.left.end()) {
return false;
}
entry_type::left_const_iterator it2 = allowed_entries_bimap_.left.find(name2);
if(it2 == allowed_entries_bimap_.left.end()) {
return false;
}
allowed_entries_[it1->second][it2->second] = allowed;
allowed_entries_[it2->second][it1->second] = allowed;
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(unsigned int i, unsigned int j,
bool allowed)
{
if(i > allowed_entries_.size() || j > allowed_entries_[i].size()) {
return false;
}
allowed_entries_[i][j] = allowed;
allowed_entries_[j][i] = allowed;
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name,
bool allowed)
{
if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end()) {
return false;
}
unsigned int ind = allowed_entries_bimap_.left.find(name)->second;
for(unsigned int i = 0; i < allowed_entries_.size(); i++) {
allowed_entries_[i][ind] = allowed;
allowed_entries_[ind][i] = allowed;
}
return true;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name,
const std::vector<std::string>& change_names,
bool allowed)
{
bool ok = true;
if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end()) {
ROS_DEBUG_STREAM("No entry for " << name);
ok = false;
return ok;
}
unsigned int ind_1 = allowed_entries_bimap_.left.find(name)->second;
for(unsigned int i = 0; i < change_names.size(); i++) {
if(allowed_entries_bimap_.left.find(change_names[i]) == allowed_entries_bimap_.left.end()) {
ROS_DEBUG_STREAM("No entry for " << change_names[i]);
ok = false;
continue;
}
unsigned int ind_2 = allowed_entries_bimap_.left.find(change_names[i])->second;
if(ind_1 >= allowed_entries_.size()) {
ROS_ERROR_STREAM("Got an index entry for name " << name << " ind " << ind_1 << " but only have "
<< allowed_entries_.size() << " in allowed collision matrix.");
return false;
}
if(ind_2 >= allowed_entries_[ind_1].size()) {
ROS_ERROR_STREAM("Got an index entry for name " << change_names[i] << " index " << ind_2 << " but only have " <<
allowed_entries_[ind_1].size() << " in allowed collision matrix.");
return false;
}
allowed_entries_[ind_1][ind_2] = allowed;
allowed_entries_[ind_2][ind_1] = allowed;
}
return ok;
}
bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::vector<std::string>& change_names_1,
const std::vector<std::string>& change_names_2,
bool allowed)
{
bool ok = true;
for(unsigned int i = 0; i < change_names_1.size(); i++) {
if(!changeEntry(change_names_1[i], change_names_2, allowed)) {
ok = false;
}
}
return ok;
}
void collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::print(std::ostream& out) const {
for(entry_type::right_const_iterator it = allowed_entries_bimap_.right.begin(); it != allowed_entries_bimap_.right.end(); it++) {
out << std::setw(40) << it->second;
out << " | ";
for(entry_type::right_const_iterator it2 = allowed_entries_bimap_.right.begin(); it2 != allowed_entries_bimap_.right.end(); it2++) {
out << std::setw(3) << allowed_entries_[it->first][it2->first];
}
out << std::endl;
}
}
bool collision_space_fcl::EnvironmentModel::getVerbose(void) const
{
return verbose_;
}
void collision_space_fcl::EnvironmentModel::setVerbose(bool verbose)
{
verbose_ = verbose;
}
const collision_space_fcl::EnvironmentObjects* collision_space_fcl::EnvironmentModel::getObjects(void) const
{
return objects_;
}
const planning_models::KinematicModel* collision_space_fcl::EnvironmentModel::getRobotModel(void) const
{
return robot_model_;
}
double collision_space_fcl::EnvironmentModel::getRobotScale(void) const
{
return robot_scale_;
}
double collision_space_fcl::EnvironmentModel::getRobotPadding(void) const
{
return default_robot_padding_;
}
void collision_space_fcl::EnvironmentModel::setRobotModel(const planning_models::KinematicModel* model,
const AllowedCollisionMatrix& acm,
const std::map<std::string, double>& link_padding_map,
double default_padding,
double scale)
{
robot_model_ = model;
default_collision_matrix_ = acm;
robot_scale_ = scale;
default_robot_padding_ = default_padding;
default_link_padding_map_ = link_padding_map;
}
void collision_space_fcl::EnvironmentModel::lock(void) const
{
lock_.lock();
}
void collision_space_fcl::EnvironmentModel::unlock(void) const
{
lock_.unlock();
}
const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix&
collision_space_fcl::EnvironmentModel::getDefaultAllowedCollisionMatrix() const
{
return default_collision_matrix_;
}
const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix&
collision_space_fcl::EnvironmentModel::getCurrentAllowedCollisionMatrix() const {
if(use_altered_collision_matrix_) {
return altered_collision_matrix_;
}
return default_collision_matrix_;
}
void collision_space_fcl::EnvironmentModel::setAlteredCollisionMatrix(const AllowedCollisionMatrix& acm) {
use_altered_collision_matrix_ = true;
altered_collision_matrix_ = acm;
}
void collision_space_fcl::EnvironmentModel::revertAlteredCollisionMatrix() {
use_altered_collision_matrix_ = false;
}
void collision_space_fcl::EnvironmentModel::setAlteredLinkPadding(const std::map<std::string, double>& new_link_padding) {
altered_link_padding_map_.clear();
for(std::map<std::string, double>::const_iterator it = new_link_padding.begin();
it != new_link_padding.end();
it++) {
if(default_link_padding_map_.find(it->first) == default_link_padding_map_.end()) {
//don't have this currently
continue;
}
//only putting in altered padding if it's different
if(default_link_padding_map_.find(it->first)->second != it->second) {
altered_link_padding_map_[it->first] = it->second;
}
}
use_altered_link_padding_map_ = true;
}
void collision_space_fcl::EnvironmentModel::revertAlteredLinkPadding() {
altered_link_padding_map_.clear();
use_altered_link_padding_map_ = false;
}
const std::map<std::string, double>& collision_space_fcl::EnvironmentModel::getDefaultLinkPaddingMap() const {
return default_link_padding_map_;
}
std::map<std::string, double> collision_space_fcl::EnvironmentModel::getCurrentLinkPaddingMap() const {
std::map<std::string, double> ret_map = default_link_padding_map_;
for(std::map<std::string, double>::const_iterator it = altered_link_padding_map_.begin();
it != altered_link_padding_map_.end();
it++) {
ret_map[it->first] = it->second;
}
return ret_map;
}
double collision_space_fcl::EnvironmentModel::getCurrentLinkPadding(std::string name) const {
if(altered_link_padding_map_.find(name) != altered_link_padding_map_.end()) {
return altered_link_padding_map_.find(name)->second;
} else if(default_link_padding_map_.find(name) != default_link_padding_map_.end()) {
return default_link_padding_map_.find(name)->second;
}
return 0.0;
}
void collision_space_fcl::EnvironmentModel::setAllowedContacts(const std::vector<AllowedContact>& allowed_contacts)
{
allowed_contact_map_.clear();
allowed_contacts_ = allowed_contacts;
for(unsigned int i = 0; i < allowed_contacts.size(); i++) {
allowed_contact_map_[allowed_contacts_[i].body_name_1][allowed_contacts_[i].body_name_2].push_back(allowed_contacts_[i]);
allowed_contact_map_[allowed_contacts_[i].body_name_2][allowed_contacts_[i].body_name_1].push_back(allowed_contacts_[i]);
}
}
const std::vector<collision_space_fcl::EnvironmentModel::AllowedContact>& collision_space_fcl::EnvironmentModel::getAllowedContacts() const {
return allowed_contacts_;
}
branches/point_cloud/collision_space_fcl/src/environment_objects.cpp deleted 100644 → 0
View file @ c45c79a7
/*********************************************************************
* 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_fcl/environment_objects.h"
#include <geometric_shapes/shape_operations.h>
std::vector<std::string> collision_space_fcl::EnvironmentObjects::getNamespaces(void) const
{
std::vector<std::string> ns;
for (std::map<std::string, NamespaceObjects>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
ns.push_back(it->first);
return ns;
}
const collision_space_fcl::EnvironmentObjects::NamespaceObjects& collision_space_fcl::EnvironmentObjects::getObjects(const std::string &ns) const
{
std::map<std::string, NamespaceObjects>::const_iterator it = objects_.find(ns);
if (it == objects_.end())
return empty_;
else
return it->second;
}
collision_space_fcl::EnvironmentObjects::NamespaceObjects& collision_space_fcl::EnvironmentObjects::getObjects(const std::string &ns)
{
return objects_[ns];
}
void collision_space_fcl::EnvironmentObjects::addObject(const std::string &ns, shapes::StaticShape *shape)
{
objects_[ns].static_shape.push_back(shape);
}
void collision_space_fcl::EnvironmentObjects::addObject(const std::string &ns, shapes::Shape *shape, const btTransform &pose)
{
objects_[ns].shape.push_back(shape);
objects_[ns].shape_pose.push_back(pose);
}
bool collision_space_fcl::EnvironmentObjects::removeObject(const std::string &ns, const shapes::Shape *shape)
{
std::map<std::string, NamespaceObjects>::iterator it = objects_.find(ns);
if (it != objects_.end())
{
unsigned int n = it->second.shape.size();
for (unsigned int i = 0 ; i < n ; ++i)
if (it->second.shape[i] == shape)
{
it->second.shape.erase(it->second.shape.begin() + i);
it->second.shape_pose.erase(it->second.shape_pose.begin() + i);
return true;
}
}
return false;
}
bool collision_space_fcl::EnvironmentObjects::removeObject(const std::string &ns, const shapes::StaticShape *shape)
{
std::map<std::string, NamespaceObjects>::iterator it = objects_.find(ns);
if (it != objects_.end())
{
unsigned int n = it->second.static_shape.size();
for (unsigned int i = 0 ; i < n ; ++i)
if (it->second.static_shape[i] == shape)
{
it->second.static_shape.erase(it->second.static_shape.begin() + i);
return true;
}
}
return false;
}
void collision_space_fcl::EnvironmentObjects::clearObjects(const std::string &ns)
{
std::map<std::string, NamespaceObjects>::iterator it = objects_.find(ns);
if (it != objects_.end())
{
unsigned int n = it->second.static_shape.size();
for (unsigned int i = 0 ; i < n ; ++i)
delete it->second.static_shape[i];
n = it->second.shape.size();
for (unsigned int i = 0 ; i < n ; ++i)
delete it->second.shape[i];
objects_.erase(it);
}
}
void collision_space_fcl::EnvironmentObjects::clearObjects(void)
{
std::vector<std::string> ns = getNamespaces();
for (unsigned int i = 0 ; i < ns.size() ; ++i)
clearObjects(ns[i]);
}
void collision_space_fcl::EnvironmentObjects::addObjectNamespace(const std::string ns)
{
if(objects_.find(ns) == objects_.end()) {
objects_[ns] = NamespaceObjects();
}
//doesn't do anything if the object is already in objects_
}
collision_space_fcl::EnvironmentObjects* collision_space_fcl::EnvironmentObjects::clone(void) const
{
EnvironmentObjects *c = new EnvironmentObjects();
for (std::map<std::string, NamespaceObjects>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
{
NamespaceObjects &ns = c->objects_[it->first];
unsigned int n = it->second.static_shape.size();
for (unsigned int i = 0 ; i < n ; ++i)
ns.static_shape.push_back(shapes::cloneShape(it->second.static_shape[i]));
n = it->second.shape.size();
for (unsigned int i = 0 ; i < n ; ++i)
ns.shape.push_back(shapes::cloneShape(it->second.shape[i]));
ns.shape_pose = it->second.shape_pose;
}
return c;
}
branches/point_cloud/collision_space_fcl/test/test_collision_space_fcl.cpp deleted 100644 → 0
View file @ c45c79a7
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, 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 Willow Garage, Inc. 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 Jia Pan */
#include <planning_models/kinematic_model.h>
#include <planning_models/kinematic_state.h>
#include <gtest/gtest.h>
#include <sstream>
#include <ctype.h>
#include <ros/package.h>
#include <collision_space_fcl/environmentFCL.h>
//urdf location relative to the planning_models path
static const std::string rel_path = "/test_urdf/robot.xml";
class TestCollisionSpaceFCL : public testing::Test {
protected:
virtual void SetUp() {
full_path_ = ros::package::getPath("planning_models")+rel_path;
urdf_ok_ = urdf_model_.initFile(full_path_);
std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
//now this should work with an identity transform
planning_models::KinematicModel::MultiDofConfig config("base_joint");
config.type = "Planar";
config.parent_frame_id = "base_footprint";
config.child_frame_id = "base_footprint";
multi_dof_configs.push_back(config);
std::vector<planning_models::KinematicModel::GroupConfig> gcs;
planning_models::KinematicModel::GroupConfig left_arm("left_arm",
"torso_lift_link",
"l_wrist_roll_link");
planning_models::KinematicModel::GroupConfig right_arm("right_arm",
"torso_lift_link",
"r_wrist_roll_link");
kinematic_model_ = new planning_models::KinematicModel(urdf_model_,
gcs,
multi_dof_configs);
coll_space_ = new collision_space_fcl::EnvironmentModelFCL();
};
virtual void TearDown() {
delete kinematic_model_;
delete coll_space_;
}
protected:
urdf::Model urdf_model_;
bool urdf_ok_;
std::string full_path_;
collision_space_fcl::EnvironmentModelFCL* coll_space_;
planning_models::KinematicModel* kinematic_model_;
};
TEST_F(TestCollisionSpaceFCL, TestInit) {
std::vector<std::string> links;
kinematic_model_->getLinkModelNames(links);
std::map<std::string, double> link_padding_map;
{
collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links,true);
coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
//all AllowedCollisions set to true, so no collision
ASSERT_FALSE(coll_space_->isCollision());
}
{
collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
//now we are in collision with nothing disabled
ASSERT_TRUE(coll_space_->isCollision());
}
//one more time for good measure
{
collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
//now we are in collision with nothing disabled
ASSERT_TRUE(coll_space_->isCollision());
}
}
TEST_F(TestCollisionSpaceFCL, TestACM) {
std::vector<std::string> links;
kinematic_model_->getLinkModelNames(links);
std::map<std::string, double> link_padding_map;
//first we get
{
collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
planning_models::KinematicState state(kinematic_model_);
state.setKinematicStateToDefault();
coll_space_->updateRobotModel(&state);
//at default state in collision
ASSERT_TRUE(coll_space_->isCollision());
//now we get the full set of collisions in the default state
std::vector<collision_space_fcl::EnvironmentModel::AllowedContact> ac;
std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
coll_space_->getAllCollisionContacts(ac, contacts, 1);
ASSERT_TRUE(contacts.size() > 1);
//now we change all these pairwise to true
for(unsigned int i = 0; i < contacts.size(); i++) {
ASSERT_TRUE(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::LINK);
ASSERT_TRUE(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::LINK);
ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
}
coll_space_->setAlteredCollisionMatrix(acm);
//with all of these disabled, no more collisions
ASSERT_FALSE(coll_space_->isCollision());
coll_space_->revertAlteredCollisionMatrix();
ASSERT_TRUE(coll_space_->isCollision());
}
}
TEST_F(TestCollisionSpaceFCL, TestAttachedObjects)
{
std::vector<std::string> links;
kinematic_model_->getLinkModelNames(links);
std::map<std::string, double> link_padding_map;
collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
{
//indented cause the state needs to cease to exist before we add the attached body
planning_models::KinematicState state(kinematic_model_);
state.setKinematicStateToDefault();
coll_space_->updateRobotModel(&state);
//now we get the full set of collisions in the default state
std::vector<collision_space_fcl::EnvironmentModel::AllowedContact> ac;
std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
coll_space_->getAllCollisionContacts(ac, contacts, 1);
//now we change all these pairwise to true
for(unsigned int i = 0; i < contacts.size(); i++) {
ASSERT_TRUE(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::LINK);
ASSERT_TRUE(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::LINK);
ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
}
coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
coll_space_->updateRobotModel(&state);
}
//now we shouldn't be in collision
ASSERT_FALSE(coll_space_->isCollision());
const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
//first a single box
shapes::Sphere* sphere1 = new shapes::Sphere();
sphere1->radius = .1;
shapes::Box* box2 = new shapes::Box();
box2->size[0] = .05;
box2->size[1] = .05;
box2->size[2] = .05;
std::vector<shapes::Shape*> shape_vector;
shape_vector.push_back(sphere1);
btTransform pose;
pose.setIdentity();
std::vector<btTransform> poses;
poses.push_back(pose);
std::vector<std::string> touch_links;
planning_models::KinematicModel::AttachedBodyModel* ab1 =
new planning_models::KinematicModel::AttachedBodyModel(link, "box_1",
poses,
touch_links,
shape_vector);
kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
coll_space_->updateAttachedBodies();
const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix& aft_attached
= coll_space_->getDefaultAllowedCollisionMatrix();
ASSERT_TRUE(aft_attached.hasEntry("box_1"));
bool allowed;
EXPECT_TRUE(aft_attached.getAllowedCollision("box_1", link->getName(), allowed));
EXPECT_FALSE(allowed);
{
//indented cause the state needs to cease to exist before we add the attached body
planning_models::KinematicState state(kinematic_model_);
state.setKinematicStateToDefault();
coll_space_->updateRobotModel(&state);
}
//now it collides
ASSERT_TRUE(coll_space_->isCollision());
kinematic_model_->clearLinkAttachedBodyModel(link->getName(), "box_1");
coll_space_->updateAttachedBodies();
ASSERT_FALSE(aft_attached.hasEntry("box_1"));
//now adding an attached object with two boxes, this time with two objects
shape_vector.clear();
shape_vector.push_back(box2);
pose.getOrigin().setX(.1);
poses.clear();
poses.push_back(pose);
touch_links.push_back("r_gripper_palm_link");
touch_links.push_back("r_gripper_r_finger_link");
touch_links.push_back("r_gripper_l_finger_link");
touch_links.push_back("r_gripper_r_finger_tip_link");
touch_links.push_back("r_gripper_l_finger_tip_link");
touch_links.push_back("base_link");
planning_models::KinematicModel::AttachedBodyModel* ab2 =
new planning_models::KinematicModel::AttachedBodyModel(link, "box_2",
poses,
touch_links,
shape_vector);
kinematic_model_->addAttachedBodyModel(link->getName(), ab2);
coll_space_->updateAttachedBodies();
ASSERT_TRUE(aft_attached.hasEntry("box_2"));
EXPECT_TRUE(aft_attached.getAllowedCollision("box_2", link->getName(), allowed));
EXPECT_TRUE(allowed);
{
//indented cause the state needs to cease to exist before we add the attached body
planning_models::KinematicState state(kinematic_model_);
state.setKinematicStateToDefault();
coll_space_->updateRobotModel(&state);
}
//now it doesn't collide
ASSERT_FALSE(coll_space_->isCollision());
}
TEST_F(TestCollisionSpaceFCL, TestStaticObjects)
{
std::vector<std::string> links;
kinematic_model_->getLinkModelNames(links);
std::map<std::string, double> link_padding_map;
collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
{
//indented cause the state needs to cease to exist before we add the attached body
planning_models::KinematicState state(kinematic_model_);
state.setKinematicStateToDefault();
coll_space_->updateRobotModel(&state);
}
ASSERT_FALSE(coll_space_->isEnvironmentCollision());
shapes::Sphere* sphere1 = new shapes::Sphere();
sphere1->radius = .2;
btTransform pose;
pose.setIdentity();
std::vector<btTransform> poses;
poses.push_back(pose);
std::vector<shapes::Shape*> shape_vector;
shape_vector.push_back(sphere1);
coll_space_->addObjects("obj1", shape_vector, poses);
ASSERT_TRUE(coll_space_->isEnvironmentCollision());
//Now test interactions between static and attached objects
const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
shapes::Box* att_box = new shapes::Box();
att_box->size[0] = .05;
att_box->size[1] = .05;
att_box->size[2] = .05;
std::vector<shapes::Shape*> att_shapes;
att_shapes.push_back(att_box);
btTransform att_pose;
att_pose.setIdentity();
std::vector<btTransform> att_poses;
att_poses.push_back(att_pose);
std::vector<std::string> touch_links;
touch_links.push_back("base_link");
touch_links.push_back("base_footprint");
planning_models::KinematicModel::AttachedBodyModel* ab1 =
new planning_models::KinematicModel::AttachedBodyModel(link, "att1",
att_poses,
touch_links,
att_shapes);
kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
coll_space_->updateAttachedBodies();
{
//indented cause the state needs to cease to exist before we add the attached body
planning_models::KinematicState state(kinematic_model_);
state.setKinematicStateToDefault();
coll_space_->updateRobotModel(&state);
}
ASSERT_TRUE(coll_space_->isEnvironmentCollision());
//now we get the full set of collisions in the default state
std::vector<collision_space_fcl::EnvironmentModel::AllowedContact> ac;
std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
coll_space_->getAllCollisionContacts(ac, contacts, 1);
//now we change all these pairwise to true
for(unsigned int i = 0; i < contacts.size(); i++) {
if(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::OBJECT) {
ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
}
if(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::OBJECT) {
ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
}
}
acm = coll_space_->getDefaultAllowedCollisionMatrix();
bool allowed;
ASSERT_TRUE(acm.getAllowedCollision("obj1","att1",allowed));
EXPECT_FALSE(allowed);
ASSERT_TRUE(acm.changeEntry(link->getName(), "obj1", true));
ASSERT_TRUE(acm.changeEntry("base_footprint", "obj1", true));
coll_space_->setAlteredCollisionMatrix(acm);
{
//indented cause the state needs to cease to exist before we add the attached body
planning_models::KinematicState state(kinematic_model_);
state.setKinematicStateToDefault();
coll_space_->updateRobotModel(&state);
}
/* Remove because sphere-box collision does not work for mesh */
// EXPECT_TRUE(coll_space_->isEnvironmentCollision());
ASSERT_TRUE(acm.changeEntry("att1", "obj1", true));
coll_space_->setAlteredCollisionMatrix(acm);
allowed = false;
ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("att1","obj1", allowed));
EXPECT_TRUE(allowed);
ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","att1", allowed));
EXPECT_TRUE(allowed);
ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("base_link","obj1", allowed));
EXPECT_TRUE(allowed);
ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_link", allowed));
EXPECT_TRUE(allowed);
ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_footprint", allowed));
EXPECT_TRUE(allowed);
EXPECT_FALSE(coll_space_->isEnvironmentCollision());
contacts.clear();
coll_space_->getAllCollisionContacts(ac, contacts, 1);
//now we change all these pairwise to true
for(unsigned int i = 0; i < contacts.size(); i++) {
if(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::OBJECT) {
ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
ROS_INFO_STREAM(contacts[i].body_name_2);
}
if(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::OBJECT) {
ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
ROS_INFO_STREAM(contacts[i].body_name_1);
}
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}