Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20206] pkg/trunk
|
From: <is...@us...> - 2009-07-30 23:31:20
|
Revision: 20206
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20206&view=rev
Author: isucan
Date: 2009-07-30 23:31:13 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
working on the ability to edit collision worlds and re-construct collision maps
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.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/environmentBullet.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/environment.cpp
pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/test_collision_space/CMakeLists.txt
pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
Added Paths:
-----------
pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h
pkg/trunk/world_models/collision_space/src/environment_objects.cpp
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -146,6 +146,8 @@
void setupCSM(void);
void updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear);
+ void collisionMapAsSpheres(const mapping_msgs::CollisionMapConstPtr &collisionMap,
+ std::vector<shapes::Shape*> &spheres, std::vector<btTransform> &poses);
void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
void collisionMapUpdateCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
void objectInMapCallback(const mapping_msgs::ObjectInMapConstPtr &objectInMap);
Modified: pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -59,7 +59,8 @@
for (unsigned int i = 0 ; i < boundingPlanes_.size() / 4 ; ++i)
{
- model->addPlane("bounds", boundingPlanes_[i * 4], boundingPlanes_[i * 4 + 1], boundingPlanes_[i * 4 + 2], boundingPlanes_[i * 4 + 3]);
+ shapes::Plane *plane = new shapes::Plane(boundingPlanes_[i * 4], boundingPlanes_[i * 4 + 1], boundingPlanes_[i * 4 + 2], boundingPlanes_[i * 4 + 3]);
+ model->addObject("bounds", plane);
ROS_INFO("Added static plane %fx + %fy + %fz + %f = 0 for model %p", boundingPlanes_[i * 4], boundingPlanes_[i * 4 + 1], boundingPlanes_[i * 4 + 2], boundingPlanes_[i * 4 + 3], model.get());
}
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -143,23 +143,16 @@
updateCollisionSpace(collisionMap, true);
}
-void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
+void planning_environment::CollisionSpaceMonitor::collisionMapAsSpheres(const mapping_msgs::CollisionMapConstPtr &collisionMap,
+ std::vector<shapes::Shape*> &spheres, std::vector<btTransform> &poses)
{
- boost::mutex::scoped_lock lock(mapUpdateLock_);
-
- int n = collisionMap->get_boxes_size();
-
- ROS_DEBUG("Received collision map with %d points that is %f seconds old", n, (ros::Time::now() - collisionMap->header.stamp).toSec());
-
- if (onBeforeMapUpdate_ != NULL)
- onBeforeMapUpdate_(collisionMap, clear);
-
// we want to make sure the frame the robot model is kept in is the same as the frame of the collisionMap
bool transform = !frame_id_.empty() && collisionMap->header.frame_id != frame_id_;
-
- ros::WallTime startTime = ros::WallTime::now();
- double *data = n > 0 ? new double[4 * n] : NULL;
-
+ const int n = collisionMap->get_boxes_size();
+
+ spheres.resize(n);
+ poses.resize(n);
+
if (transform)
{
std::string target = frame_id_;
@@ -168,7 +161,6 @@
#pragma omp parallel for
for (int i = 0 ; i < n ; ++i)
{
- int i4 = i * 4;
robot_msgs::PointStamped psi;
psi.header = collisionMap->header;
psi.point.x = collisionMap->boxes[i].center.x;
@@ -186,11 +178,9 @@
pso = psi;
}
- data[i4 ] = pso.point.x;
- data[i4 + 1] = pso.point.y;
- data[i4 + 2] = pso.point.z;
-
- data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_;
+ poses[i].setIdentity();
+ poses[i].setOrigin(btVector3(pso.point.x, pso.point.y, pso.point.z));
+ spheres[i] = new shapes::Sphere(maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_);
}
if (err)
@@ -202,27 +192,34 @@
#pragma omp parallel for
for (int i = 0 ; i < n ; ++i)
{
- int i4 = i * 4;
- data[i4 ] = collisionMap->boxes[i].center.x;
- data[i4 + 1] = collisionMap->boxes[i].center.y;
- data[i4 + 2] = collisionMap->boxes[i].center.z;
-
- data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_;
+ poses[i].setIdentity();
+ poses[i].setOrigin(btVector3(collisionMap->boxes[i].center.x, collisionMap->boxes[i].center.y, collisionMap->boxes[i].center.z));
+ spheres[i] = new shapes::Sphere(maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_);
}
}
+}
+
+void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
+{
+ boost::mutex::scoped_lock lock(mapUpdateLock_);
- collisionSpace_->lock();
+ ROS_DEBUG("Received collision map with %d points that is %f seconds old", collisionMap->get_boxes_size(), (ros::Time::now() - collisionMap->header.stamp).toSec());
+
+ if (onBeforeMapUpdate_ != NULL)
+ onBeforeMapUpdate_(collisionMap, clear);
- if (clear)
- collisionSpace_->clearObstacles("points");
- if (n > 0)
- collisionSpace_->addPointCloudSpheres("points", n, data);
+ ros::WallTime startTime = ros::WallTime::now();
+
+ std::vector<shapes::Shape*> spheres;
+ std::vector<btTransform> poses;
+ collisionMapAsSpheres(collisionMap, spheres, poses);
+ collisionSpace_->lock();
+ if (clear)
+ collisionSpace_->clearObjects("points");
+ collisionSpace_->addObjects("points", spheres, poses);
collisionSpace_->unlock();
- if (data)
- delete[] data;
-
double tupd = (ros::WallTime::now() - startTime).toSec();
ROS_DEBUG("Updated map model in %f seconds", tupd);
@@ -265,19 +262,18 @@
btTransform pose;
tf::poseMsgToTF(pso.pose, pose);
collisionSpace_->lock();
- collisionSpace_->clearObstacles(objectInMap->id);
+ collisionSpace_->clearObjects(objectInMap->id);
collisionSpace_->addObject(objectInMap->id, shape, pose);
collisionSpace_->unlock();
ROS_INFO("Added object '%s' to collision space", objectInMap->id.c_str());
}
- delete shape;
}
}
else
{
// remove the object from the map
collisionSpace_->lock();
- collisionSpace_->clearObstacles(objectInMap->id);
+ collisionSpace_->clearObjects(objectInMap->id);
collisionSpace_->unlock();
ROS_INFO("Removed object '%s' from collision space", objectInMap->id.c_str());
}
Modified: pkg/trunk/world_models/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-07-30 23:31:13 UTC (rev 20206)
@@ -6,7 +6,8 @@
rospack_add_boost_directories()
-rospack_add_library(collision_space src/environment.cpp
+rospack_add_library(collision_space src/environment_objects.cpp
+ src/environment.cpp
src/environmentBullet.cpp
src/environmentODE.cpp)
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -37,6 +37,7 @@
#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_
#define COLLISION_SPACE_ENVIRONMENT_MODEL_
+#include "collision_space/environment_objects.h"
#include <planning_models/kinematic.h>
#include <planning_models/output.h>
#include <LinearMath/btVector3.h>
@@ -49,6 +50,7 @@
/** \brief Main namespace */
namespace collision_space
{
+
/** \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
@@ -78,10 +80,13 @@
{
m_selfCollision = true;
m_verbose = false;
+ m_objects = new EnvironmentObjects();
}
virtual ~EnvironmentModel(void)
{
+ if (m_objects)
+ delete m_objects;
}
/**********************************************************************/
@@ -95,7 +100,7 @@
bool getSelfCollision(void) const;
/** \brief Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(std::vector<std::string> &links);
+ virtual void addSelfCollisionGroup(const std::vector<std::string> &links);
/** \brief Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
virtual int setCollisionCheck(const std::string &link, bool state) = 0;
@@ -106,7 +111,7 @@
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 boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Get robot scale */
double getRobotScale(void) const
@@ -127,7 +132,7 @@
virtual void updateAttachedBodies(void) = 0;
/** \brief Get the robot model */
- boost::shared_ptr<planning_models::KinematicModel> getRobotModel(void) const
+ const boost::shared_ptr<const planning_models::KinematicModel>& getRobotModel(void) const
{
return m_robotModel;
}
@@ -152,20 +157,23 @@
/* Collision Bodies */
/**********************************************************************/
- /** \brief Remove all obstacles from collision model */
- virtual void clearObstacles(void) = 0;
+ /** \brief Remove all objects from collision model */
+ virtual void clearObjects(void) = 0;
- /** \brief Remove obstacles from a specific namespace in the collision model */
- virtual void clearObstacles(const std::string &ns) = 0;
+ /** \brief Remove objects from a specific namespace in the collision model */
+ virtual void clearObjects(const std::string &ns) = 0;
+
+ /** \brief Add a static collision object to the map. The user releases ownership of the passed object. */
+ virtual void addObject(const std::string &ns, const shapes::StaticShape *shape) = 0;
- /** \brief Add a point cloud to the collision space */
- virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double* points) = 0;
+ /** \brief Add a collision object to the map. The user releases ownership of the passed object.*/
+ virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose) = 0;
- /** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addPlane(const std::string &ns, double a, double b, double c, double d) = 0;
+ /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. */
+ virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses) = 0;
- /** \brief Add a collision object to the map */
- virtual void addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose) = 0;
+ /** \brief Get the objects currently contained in the model */
+ const EnvironmentObjects* getObjects(void) const;
/**********************************************************************/
/* Miscellaneous Routines */
@@ -188,22 +196,41 @@
protected:
- boost::mutex m_lock;
- std::vector<std::string> m_collisionLinks;
- std::map<std::string, unsigned int> m_collisionLinkIndex;
- std::vector< std::vector<bool> > m_selfCollisionTest;
+ /** \brief Mutex used to lock the datastructure */
+ boost::mutex m_lock;
+
+ /** \brief List of links (names) from the robot model that are considered for collision checking */
+ std::vector<std::string> m_collisionLinks;
+
+ /** \brief Map used internally to find the index of a link that we do collision checking for */
+ std::map<std::string, unsigned int> m_collisionLinkIndex;
+
+ /** \brief Matrix of booleans indicating whether pairs of links can self collide */
+ std::vector< std::vector<bool> > m_selfCollisionTest;
- bool m_selfCollision;
- bool m_verbose;
- planning_models::msg::Interface m_msg;
+ /** \brief Flag to indicate whether self collision checking is enabled */
+ bool m_selfCollision;
- /** \brief List of loaded robot models */
- boost::shared_ptr<planning_models::KinematicModel> m_robotModel;
- double m_robotScale;
- double m_robotPadd;
+ /** \brief Flag to indicate whether verbose mode is on */
+ bool m_verbose;
+
+ /** \brief Interface to printing information */
+ planning_models::msg::Interface m_msg;
+ /** \brief Loaded robot model */
+ boost::shared_ptr<const planning_models::KinematicModel> m_robotModel;
+
+ /** \brief List of objects contained in the environment */
+ EnvironmentObjects *m_objects;
+
+ /** \brief Scaling used for robot links */
+ double m_robotScale;
+
+ /** \brief Padding used for robot links */
+ double m_robotPadd;
+
};
}
#endif
-
+
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -73,29 +73,29 @@
/** \brief Check if a model is in self collision */
virtual bool isSelfCollision(void);
+
+ /** \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 Add a static collision object to the map. The user releases ownership of the passed object. */
+ virtual void addObject(const std::string &ns, const shapes::StaticShape *shape);
- /** \brief Remove all obstacles from collision model */
- virtual void clearObstacles(void);
+ /** \brief Add a collision object to the map. The user releases ownership of the passed object.*/
+ virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose);
- /** \brief Remove obstacles from a specific namespace in the collision model */
- virtual void clearObstacles(const std::string &ns);
+ /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. */
+ virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses);
- /** \brief Add a point cloud to the collision space */
- virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points);
-
- /** \brief Add a collision object to the map */
- virtual void addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose);
-
- /** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addPlane(const std::string &ns, double a, double b, double c, double d);
-
/** \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 boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void);
@@ -232,6 +232,8 @@
};
btCollisionObject* createCollisionBody(const shapes::Shape *shape, double scale, double padding);
+ btCollisionObject* createCollisionBody(const shapes::StaticShape *shape);
+
void freeMemory(void);
SelfCollisionFilterCallback m_selfCollisionFilterCallback;
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -63,29 +63,29 @@
/** \brief Check if a model is in self collision */
virtual bool isSelfCollision(void);
+
+ /** \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 Add a static collision object to the map. The user releases ownership of the passed object. */
+ virtual void addObject(const std::string &ns, const shapes::StaticShape *shape);
- /** \brief Remove all obstacles from collision model */
- virtual void clearObstacles(void);
+ /** \brief Add a collision object to the map. The user releases ownership of the passed object.*/
+ virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose);
- /** \brief Remove obstacles from a specific namespace in the collision model */
- virtual void clearObstacles(const std::string &ns);
+ /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. */
+ virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses);
- /** \brief Add a point cloud to the collision space */
- virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points);
-
- /** \brief Add a collision object to the map */
- virtual void addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose);
-
- /** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addPlane(const std::string &ns, double a, double b, double c, double d);
-
/** \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 boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void);
@@ -272,13 +272,13 @@
storage.clear();
}
- std::string name;
+ std::string name;
dSpaceID space;
std::vector<dGeomID> geoms;
ODECollide2 collide2;
ODEStorage storage;
};
-
+
struct CollisionData
{
CollisionData(void)
@@ -315,6 +315,7 @@
dGeomID copyGeom(dSpaceID space, ODEStorage &storage, dGeomID geom, ODEStorage &sourceStorage) const;
void createODERobotModel(void);
dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::Shape *shape, double scale, double padding);
+ dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape);
void updateGeom(dGeomID geom, const btTransform &pose) const;
/** \brief Check if thread-specific routines have been called */
Added: pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h (rev 0)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -0,0 +1,105 @@
+/*********************************************************************
+* 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_ENVIRONMENT_OBJECTS_
+#define COLLISION_SPACE_ENVIRONMENT_OBJECTS_
+
+#include <vector>
+#include <string>
+#include <map>
+
+#include <geometric_shapes/shapes.h>
+#include <LinearMath/btTransform.h>
+
+namespace collision_space
+{
+
+ /** \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< const shapes::StaticShape* > staticShape;
+
+ /** \brief An array of shapes */
+ std::vector< const shapes::Shape* > shape;
+
+ /** \brief An array of shape poses */
+ std::vector< btTransform > shapePose;
+ };
+
+ /** \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 Add a static object to the namespace. The user releases ownership of the object. */
+ void addObject(const std::string &ns, const shapes::StaticShape *shape);
+
+ /** \brief Add an object to the namespace. The user releases ownership of the object. */
+ void addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose);
+
+ /** \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 Clone this instance of the class */
+ EnvironmentObjects* clone(void) const;
+
+ private:
+
+ std::map<std::string, NamespaceObjects> m_objects;
+ NamespaceObjects m_empty;
+ };
+
+}
+
+#endif
+
Modified: pkg/trunk/world_models/collision_space/src/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -46,8 +46,13 @@
m_verbose = verbose;
}
-void collision_space::EnvironmentModel::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+const collision_space::EnvironmentObjects* collision_space::EnvironmentModel::getObjects(void) const
{
+ return m_objects;
+}
+
+void collision_space::EnvironmentModel::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+{
m_robotModel = model;
m_collisionLinks = links;
m_selfCollisionTest.resize(links.size());
@@ -60,7 +65,7 @@
m_robotPadd = padding;
}
-void collision_space::EnvironmentModel::addSelfCollisionGroup(std::vector<std::string> &links)
+void collision_space::EnvironmentModel::addSelfCollisionGroup(const std::vector<std::string> &links)
{
for (unsigned int i = 0 ; i < links.size() ; ++i)
{
Modified: pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -59,7 +59,7 @@
delete m_config; */
}
-void collision_space::EnvironmentModelBullet::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModelBullet::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
collision_space::EnvironmentModel::setRobotModel(model, links, scale, padding);
m_modelGeom.scale = scale;
@@ -145,6 +145,33 @@
return NULL;
}
+btCollisionObject* collision_space::EnvironmentModelBullet::createCollisionBody(const shapes::StaticShape *shape)
+{
+ btCollisionShape *btshape = NULL;
+
+ switch (shape->type)
+ {
+ case shapes::PLANE:
+ {
+ const shapes::Plane *p = static_cast<const shapes::Plane*>(shape);
+ btshape = new btStaticPlaneShape(btVector3(p->a, p->b, p->c), p->d);
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ if (btshape)
+ {
+ btCollisionObject *object = new btCollisionObject();
+ object->setCollisionShape(btshape);
+ return object;
+ }
+ else
+ return NULL;
+}
+
void collision_space::EnvironmentModelBullet::updateAttachedBodies(void)
{
const unsigned int n = m_modelGeom.linkGeom.size();
@@ -265,7 +292,7 @@
return false;
}
-
+/*
void collision_space::EnvironmentModelBullet::addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points)
{
btTransform t;
@@ -283,25 +310,34 @@
m_obstacles[ns].push_back(object);
}
}
+*/
-void collision_space::EnvironmentModelBullet::addPlane(const std::string &ns, double a, double b, double c, double d)
+
+void collision_space::EnvironmentModelBullet::addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses)
{
- btCollisionObject *object = new btCollisionObject();
- btCollisionShape *shape = new btStaticPlaneShape(btVector3(a, b, c), d);
- object->setCollisionShape(shape);
- m_world->addCollisionObject(object);
- m_obstacles[ns].push_back(object);
+ assert(shapes.size() == poses.size());
+ for (unsigned int i = 0 ; i < shapes.size() ; ++i)
+ addObject(ns, shapes[i], poses[i]);
}
+void collision_space::EnvironmentModelBullet::addObject(const std::string &ns, const shapes::StaticShape* shape)
+{
+ btCollisionObject *obj = createCollisionBody(shape);
+ m_world->addCollisionObject(obj);
+ m_obstacles[ns].push_back(obj);
+ m_objects->addObject(ns, shape);
+}
+
void collision_space::EnvironmentModelBullet::addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose)
{
btCollisionObject *obj = createCollisionBody(shape, 1.0, 0.0);
obj->setWorldTransform(pose);
m_world->addCollisionObject(obj);
m_obstacles[ns].push_back(obj);
+ m_objects->addObject(ns, shape, pose);
}
-void collision_space::EnvironmentModelBullet::clearObstacles(const std::string &ns)
+void collision_space::EnvironmentModelBullet::clearObjects(const std::string &ns)
{
if (m_obstacles.find(ns) != m_obstacles.end())
{
@@ -314,13 +350,14 @@
// delete obj;
}
m_obstacles.erase(ns);
- }
+ }
+ m_objects->clearObjects(ns);
}
-void collision_space::EnvironmentModelBullet::clearObstacles(void)
+void collision_space::EnvironmentModelBullet::clearObjects(void)
{
for (std::map<std::string, std::vector<btCollisionObject*> >::iterator it = m_obstacles.begin() ; it != m_obstacles.end() ; ++it)
- clearObstacles(it->first);
+ clearObjects(it->first);
}
int collision_space::EnvironmentModelBullet::setCollisionCheck(const std::string &link, bool state)
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -60,6 +60,8 @@
ODEInitCountLock.unlock();
checkThreadInit();
+
+ m_modelGeom.space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY);
}
collision_space::EnvironmentModelODE::~EnvironmentModelODE(void)
@@ -98,7 +100,7 @@
delete it->second;
}
-void collision_space::EnvironmentModelODE::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModelODE::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
collision_space::EnvironmentModel::setRobotModel(model, links, scale, padding);
createODERobotModel();
@@ -106,8 +108,6 @@
void collision_space::EnvironmentModelODE::createODERobotModel(void)
{
- m_modelGeom.space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY);
-
for (unsigned int i = 0 ; i < m_collisionLinks.size() ; ++i)
{
/* skip this link if we have no geometry or if the link
@@ -136,6 +136,23 @@
}
}
+dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape)
+{
+ dGeomID g = NULL;
+ switch (shape->type)
+ {
+ case shapes::PLANE:
+ {
+ const shapes::Plane *p = static_cast<const shapes::Plane*>(shape);
+ g = dCreatePlane(space, p->a, p->b, p->c, p->d);
+ }
+ break;
+ default:
+ break;
+ }
+ return g;
+}
+
dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::Shape *shape, double scale, double padding)
{
dGeomID g = NULL;
@@ -587,8 +604,9 @@
}
}
-void collision_space::EnvironmentModelODE::addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points)
+void collision_space::EnvironmentModelODE::addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses)
{
+ assert(shapes.size() == poses.size());
std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.find(ns);
CollisionNamespace* cn = NULL;
if (it == m_collNs.end())
@@ -598,12 +616,15 @@
}
else
cn = it->second;
+
+ unsigned int n = shapes.size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- unsigned int i4 = i * 4;
- dGeomID g = dCreateSphere(cn->space, points[i4 + 3]);
- dGeomSetPosition(g, points[i4], points[i4 + 1], points[i4 + 2]);
+ dGeomID g = createODEGeom(cn->space, cn->storage, shapes[i], 1.0, 0.0);
+ assert(g);
+ updateGeom(g, poses[i]);
cn->collide2.registerGeom(g);
+ m_objects->addObject(ns, shapes[i], poses[i]);
}
cn->collide2.setup();
}
@@ -621,11 +642,13 @@
cn = it->second;
dGeomID g = createODEGeom(cn->space, cn->storage, shape, 1.0, 0.0);
+ assert(g);
updateGeom(g, pose);
cn->geoms.push_back(g);
+ m_objects->addObject(ns, shape, pose);
}
-void collision_space::EnvironmentModelODE::addPlane(const std::string &ns, double a, double b, double c, double d)
+void collision_space::EnvironmentModelODE::addObject(const std::string &ns, const shapes::StaticShape* shape)
{
std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.find(ns);
CollisionNamespace* cn = NULL;
@@ -636,22 +659,27 @@
}
else
cn = it->second;
- dGeomID g = dCreatePlane(cn->space, a, b, c, d);
+
+ dGeomID g = createODEGeom(cn->space, cn->storage, shape);
+ assert(g);
cn->geoms.push_back(g);
+ m_objects->addObject(ns, shape);
}
-void collision_space::EnvironmentModelODE::clearObstacles(void)
+void collision_space::EnvironmentModelODE::clearObjects(void)
{
for (std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.begin() ; it != m_collNs.end() ; ++it)
delete it->second;
m_collNs.clear();
+ m_objects->clearObjects();
}
-void collision_space::EnvironmentModelODE::clearObstacles(const std::string &ns)
+void collision_space::EnvironmentModelODE::clearObjects(const std::string &ns)
{
std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.find(ns);
if (it != m_collNs.end())
it->second->clear();
+ m_objects->clearObjects(ns);
}
int collision_space::EnvironmentModelODE::setCollisionCheck(const std::string &link, bool state)
@@ -753,7 +781,7 @@
env->m_verbose = m_verbose;
env->m_robotScale = m_robotScale;
env->m_robotPadd = m_robotPadd;
- env->m_robotModel = boost::shared_ptr<planning_models::KinematicModel>(m_robotModel->clone());
+ env->m_robotModel = boost::shared_ptr<const planning_models::KinematicModel>(m_robotModel->clone());
env->createODERobotModel();
for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
env->m_modelGeom.linkGeom[j]->enabled = m_modelGeom.linkGeom[j]->enabled;
@@ -771,6 +799,8 @@
for (unsigned int i = 0 ; i < n ; ++i)
cn->collide2.registerGeom(copyGeom(cn->space, cn->storage, geoms[i], it->second->storage));
}
-
+ if (env->m_objects)
+ delete env->m_objects;
+ env->m_objects = m_objects->clone();
return env;
}
Added: pkg/trunk/world_models/collision_space/src/environment_objects.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment_objects.cpp (rev 0)
+++ pkg/trunk/world_models/collision_space/src/environment_objects.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -0,0 +1,104 @@
+/*********************************************************************
+* 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_objects.h"
+
+std::vector<std::string> collision_space::EnvironmentObjects::getNamespaces(void) const
+{
+ std::vector<std::string> ns;
+ for (std::map<std::string, NamespaceObjects>::const_iterator it = m_objects.begin() ; it != m_objects.end() ; ++it)
+ ns.push_back(it->first);
+ return ns;
+}
+
+const collision_space::EnvironmentObjects::NamespaceObjects& collision_space::EnvironmentObjects::getObjects(const std::string &ns) const
+{
+ std::map<std::string, NamespaceObjects>::const_iterator it = m_objects.find(ns);
+ if (it == m_objects.end())
+ return m_empty;
+ else
+ return it->second;
+}
+
+void collision_space::EnvironmentObjects::addObject(const std::string &ns, const shapes::StaticShape *shape)
+{
+ m_objects[ns].staticShape.push_back(shape);
+}
+
+void collision_space::EnvironmentObjects::addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose)
+{
+ m_objects[ns].shape.push_back(shape);
+ m_objects[ns].shapePose.push_back(pose);
+}
+
+void collision_space::EnvironmentObjects::clearObjects(const std::string &ns)
+{
+ std::map<std::string, NamespaceObjects>::iterator it = m_objects.find(ns);
+ if (it != m_objects.end())
+ {
+ unsigned int n = it->second.staticShape.size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ delete it->second.staticShape[i];
+ n = it->second.shape.size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ delete it->second.shape[i];
+ m_objects.erase(it);
+ }
+}
+
+void collision_space::EnvironmentObjects::clearObjects(void)
+{
+ std::vector<std::string> ns = getNamespaces();
+ for (unsigned int i = 0 ; i < ns.size() ; ++i)
+ clearObjects(ns[i]);
+}
+
+collision_space::EnvironmentObjects* collision_space::EnvironmentObjects::clone(void) const
+{
+ EnvironmentObjects *c = new EnvironmentObjects();
+ for (std::map<std::string, NamespaceObjects>::const_iterator it = m_objects.begin() ; it != m_objects.end() ; ++it)
+ {
+ NamespaceObjects &ns = c->m_objects[it->first];
+ unsigned int n = it->second.staticShape.size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ ns.staticShape.push_back(shapes::clone_shape(it->second.staticShape[i]));
+ n = it->second.shape.size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ ns.shape.push_back(shapes::clone_shape(it->second.shape[i]));
+ ns.shapePose = it->second.shapePose;
+ }
+ return c;
+}
Modified: pkg/trunk/world_models/test_collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/test_collision_space/CMakeLists.txt 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/test_collision_space/CMakeLists.txt 2009-07-30 23:31:13 UTC (rev 20206)
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-set(ROS_BUILD_TYPE Release)
+set(ROS_BUILD_TYPE Debug)
rospack_add_boost_directories()
Modified: pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
===================================================================
--- pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -67,19 +67,20 @@
ROS_INFO("Collision (should be 0): %d", em->isCollision());
const int n = 10000;
- double *data = new double[n * 4];
+ std::vector<shapes::Shape*> spheres;
+ std::vector<btTransform> poses;
+
for (int i = 0 ; i < n ; ++i)
{
- int i4 = i * 4;
+ btTransform pos;
+ pos.setIdentity();
do
{
- data[i4 + 0] = uniform(1.5);
- data[i4 + 1] = uniform(1.5);
- data[i4 + 2] = uniform(1.5);
- data[i4 + 3] = 0.02;
- em->clearObstacles();
- em->addPointCloudSpheres("points", 1, data + i4);
+ shapes::Sphere* obj = new shapes::Sphere(0.02);
+ em->clearObjects();
+ pos.setOrigin(btVector3(uniform(1.5), uniform(1.5), uniform(1.5)));
+ em->addObject("points", obj, pos);
if (n < 100)
{
collision_space::EnvironmentModel *clone = em->clone();
@@ -90,15 +91,20 @@
}
}
while(em->isCollision());
+ spheres.push_back(new shapes::Sphere(0.02));
+ poses.push_back(pos);
}
- em->clearObstacles();
- em->addPointCloudSpheres("points", n, data);
- ROS_INFO("Added %d points", n);
+ em->clearObjects();
+ em->addObjects("points", spheres, poses);
+ ROS_INFO("Added %d spheres", n);
- delete[] data;
-
ROS_INFO("Collision (should be 0): %d", em->isCollision());
+ collision_space::EnvironmentModel *clone = em->clone();
+ clone->updateRobotModel();
+ clone->setVerbose(true);
+ ROS_INFO("Collision of clone (should be 0): %d", clone->isCollision());
+ delete clone;
const unsigned int K = 10000;
@@ -130,46 +136,45 @@
}
ROS_INFO("%f collision tests + clones per second", (double)C/(ros::WallTime::now() - tm).toSec());
}
-
+
void testCollision(void)
{
if (!cm_->loadedModels())
return;
- collision_space::EnvironmentModel *em = cm_->getODECollisionModel().get();
+ collision_space::EnvironmentModel *em = cm_->getODECollisionModel()->clone();
em->setSelfCollision(false);
em->updateRobotModel();
ROS_INFO("Collision (should be 0): %d", em->isCollision());
const int n = 10000;
- double *data = new double[n * 4];
for (int i = 0 ; i < n ; ++i)
{
- int i4 = i * 4;
+ btTransform pos;
+ pos.setIdentity();
do
- {
- data[i4 + 0] = uniform(1.5);
- data[i4 + 1] = uniform(1.5);
- data[i4 + 2] = uniform(1.5);
- data[i4 + 3] = 0.02;
- em->clearObstacles();
- em->addPointCloudSpheres("points", 1, data + i4);
+ { shapes::Sphere* obj = new shapes::Sphere(0.02);
+ em->clearObjects();
+ pos.setOrigin(btVector3(uniform(1.5), uniform(1.5), uniform(1.5)));
+ em->addObject("points", obj, pos);
}
while(!em->isCollision());
- sendPoint(data[i4], data[i4 + 1], data[i4 + 2]);
+ sendPoint(pos.getOrigin().x(), pos.getOrigin().y(), pos.getOrigin().z());
}
ROS_INFO("Added %d points", n);
- delete[] data;
+ delete em;
}
void collisionThread(int tid, collision_space::EnvironmentModel *emx)
{
collision_space::EnvironmentModel *em = emx->clone();
- ROS_INFO("Thread %d using instance %p", tid, em);
+ em->updateRobotModel();
+ ROS_INFO("Thread %d using instance %p, collision = %d (should be 0)", tid, em, em->isCollision());
+
const unsigned int K = 10000;
sleep(1);
@@ -178,34 +183,37 @@
for (unsigned int i = 0 ; i < K ; ++i)
em->isCollision();
ROS_INFO("Thread %d: %f collision tests per second (with self collision checking)", tid, (double)K/(ros::WallTime::now() - tm).toSec());
+
delete em;
}
void collisionSetupThread(collision_space::EnvironmentModel *em)
{
const int n = 10000;
- double *data = new double[n * 4];
+ std::vector<shapes::Shape*> spheres;
+ std::vector<btTransform> poses;
+
for (int i = 0 ; i < n ; ++i)
{
- int i4 = i * 4;
+ btTransform pos;
+ pos.setIdentity();
do
- {
- data[i4 + 0] = uniform(1.5);
- data[i4 + 1] = uniform(1.5);
- data[i4 + 2] = uniform(1.5);
- data[i4 + 3] = 0.02;
- em->clearObstacles();
- em->addPointCloudSpheres("points", 1, data + i4);
+ { shapes::Sphere* obj = new shapes::Sphere(0.02);
+ em->clearObjects();
+ pos.setOrigin(btVector3(uniform(1.5), uniform(1.5), uniform(1.5)));
+ em->addObject("points", obj, pos);
}
while(em->isCollision());
+ spheres.push_back(new shapes::Sphere(0.02));
+ poses.push_back(pos);
}
- em->clearObstacles();
- em->addPointCloudSpheres("points", n, data);
- ROS_INFO("Added %d points", n);
-
- delete[] data;
+ em->clearObjects();
+ em->addObjects("points", spheres, poses);
+ ROS_INFO("Added %d points", n);
+ ROS_INFO("Collision after thread setup (should be 0): %d", em->isCollision());
+
}
void testThreads(void)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
