Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20244] pkg/trunk
|
From: <is...@us...> - 2009-07-31 04:11:10
|
Revision: 20244
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20244&view=rev
Author: isucan
Date: 2009-07-31 04:10:57 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
collision space editing is now possible
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch
pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml
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/include/collision_space/environment_objects.h
pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/collision_space/src/environment_objects.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/launch/
pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch
pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch
pkg/trunk/motion_planning/planning_environment/launch/view_state_validity.launch
pkg/trunk/motion_planning/planning_environment/src/examples/
pkg/trunk/motion_planning/planning_environment/src/examples/remove_object_example.cpp
Removed Paths:
-------------
pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch
pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch
pkg/trunk/motion_planning/planning_environment/view_state_validity.launch
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-31 04:10:57 UTC (rev 20244)
@@ -36,6 +36,12 @@
rospack_link_boost(display_planner_collision_model thread)
rospack_add_openmp_flags(display_planner_collision_model)
+# Examples
+rospack_add_executable(remove_object_example src/examples/remove_object_example.cpp)
+target_link_libraries(remove_object_example planning_environment)
+rospack_link_boost(remove_object_example thread)
+rospack_add_openmp_flags(remove_object_example)
+
# Tests
# Create a model of the PR2
Deleted: pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -1,10 +0,0 @@
-
-<launch>
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="cloud_in" to="full_cloud_annotated" />
- <remap from="cloud_out" to="full_cloud_without_known_objects" />
- <param name="fixed_frame" type="string" value="/base_link" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.03" />
- </node>
-</launch>
Deleted: pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -1,8 +0,0 @@
-
-<launch>
- <node pkg="planning_environment" type="display_planner_collision_model" respawn="false" output="screen">
- <remap from="collision_map" to="collision_map_occ" />
- <remap from="collision_map_update" to="collision_map_occ_update" />
- <param name="skip_collision_map" type="bool" value="true" />
- </node>
-</launch>
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-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-31 04:10:57 UTC (rev 20244)
@@ -142,12 +142,18 @@
return pointcloud_padd_;
}
+ /** \brief If the used modified some instance of an environment model, this function provides the means to obtain a collision map (the set of boxes)
+ * from that environment model */
+ void recoverCollisionMap(const collision_space::EnvironmentModel *env, mapping_msgs::CollisionMap &cmap);
+
protected:
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 collisionMapAsBoxes(const mapping_msgs::CollisionMapConstPtr &collisionMap,
+ std::vector<shapes::Shape*> &boxes, 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);
Copied: pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch (from rev 20207, pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -0,0 +1,10 @@
+
+<launch>
+ <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <remap from="cloud_in" to="full_cloud_annotated" />
+ <remap from="cloud_out" to="full_cloud_without_known_objects" />
+ <param name="fixed_frame" type="string" value="/base_link" />
+ <param name="object_scale" type="double" value="1.0" />
+ <param name="object_padd" type="double" value="0.03" />
+ </node>
+</launch>
Property changes on: pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/motion_planning/planning_environment/clear_known_objects.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch (from rev 20207, pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -0,0 +1,8 @@
+
+<launch>
+ <node pkg="planning_environment" type="display_planner_collision_model" respawn="false" output="screen">
+ <remap from="collision_map" to="collision_map_occ" />
+ <remap from="collision_map_update" to="collision_map_occ_update" />
+ <param name="skip_collision_map" type="bool" value="true" />
+ </node>
+</launch>
Property changes on: pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/motion_planning/planning_environment/display_planner_collision_model.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/motion_planning/planning_environment/launch/view_state_validity.launch (from rev 20207, pkg/trunk/motion_planning/planning_environment/view_state_validity.launch)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/launch/view_state_validity.launch (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/launch/view_state_validity.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -0,0 +1,10 @@
+<launch>
+ <node pkg="planning_environment" type="view_state_validity" respawn="false" output="screen">
+ <remap from="collision_map" to="collision_map_occ" />
+ <remap from="collision_map_update" to="collision_map_occ_update" />
+ <param name="refresh_interval_collision_map" type="double" value="5.0" />
+ <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
+ <param name="refresh_interval_pose" type="double" value="1.0" />
+ <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
+ </node>
+</launch>
Copied: pkg/trunk/motion_planning/planning_environment/src/examples/remove_object_example.cpp (from rev 20207, pkg/trunk/motion_planning/planning_environment/src/tools/view_state_validity.cpp)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/examples/remove_object_example.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/examples/remove_object_example.cpp 2009-07-31 04:10:57 UTC (rev 20244)
@@ -0,0 +1,134 @@
+/*********************************************************************
+* 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 */
+
+/**
+
+@b RemoveObjectExample is a node that forwards a collision map after it removes a box from it
+
+**/
+
+#include "planning_environment/monitors/planning_monitor.h"
+#include <mapping_msgs/CollisionMap.h>
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <map>
+
+class RemoveObjectExample
+{
+public:
+
+ RemoveObjectExample(void)
+ {
+ collisionModels_ = new planning_environment::CollisionModels("robot_description");
+ if (collisionModels_->loadedModels())
+ {
+ collisionMapPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_with_removed_box", 1);
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
+ planningMonitor_->setOnAfterMapUpdateCallback(boost::bind(&RemoveObjectExample::afterWorldUpdate, this, _1, _2));
+ }
+ }
+
+ virtual ~RemoveObjectExample(void)
+ {
+ if (planningMonitor_)
+ delete planningMonitor_;
+ if (collisionModels_)
+ delete collisionModels_;
+ }
+
+protected:
+
+ void afterWorldUpdate(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
+ {
+ // we do not care about incremental updates, only re-writes of the map
+ if (!clear)
+ return;
+
+ // at this point, the environment model has the collision map inside it
+
+
+ // get exclusive access
+ planningMonitor_->getEnvironmentModel()->lock();
+
+ // get a copy of our own, to play with :)
+ collision_space::EnvironmentModel *env = planningMonitor_->getEnvironmentModel()->clone();
+
+ // release our hold
+ planningMonitor_->getEnvironmentModel()->unlock();
+
+
+ // create a box
+ shapes::Shape *box = new shapes::Box(2, 2, 2);
+ btTransform pose;
+ pose.setIdentity();
+
+ // remove the objects colliding with the box
+ env->removeCollidingObjects(box, pose);
+
+ // forward the updated map
+ mapping_msgs::CollisionMap cmap;
+ planningMonitor_->recoverCollisionMap(env, cmap);
+ collisionMapPublisher_.publish(cmap);
+
+ // throw away our copy
+ delete env;
+
+ ROS_INFO("Received collision map with %d points and published one with %d points",
+ (int)collisionMap->get_boxes_size(), (int)cmap.get_boxes_size());
+
+ }
+
+private:
+
+ ros::NodeHandle nh_;
+ tf::TransformListener tf_;
+
+ ros::Publisher collisionMapPublisher_;
+ planning_environment::CollisionModels *collisionModels_;
+ planning_environment::PlanningMonitor *planningMonitor_;
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "view_state_validity");
+
+ RemoveObjectExample example;
+ ros::spin();
+
+ return 0;
+}
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-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-31 04:10:57 UTC (rev 20244)
@@ -199,6 +199,64 @@
}
}
+void planning_environment::CollisionSpaceMonitor::collisionMapAsBoxes(const mapping_msgs::CollisionMapConstPtr &collisionMap,
+ std::vector<shapes::Shape*> &boxes, std::vector<btTransform> &poses)
+{
+ // 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_;
+ const int n = collisionMap->get_boxes_size();
+
+ double pd = 2.0 * pointcloud_padd_;
+
+ boxes.resize(n);
+ poses.resize(n);
+
+ if (transform)
+ {
+ std::string target = frame_id_;
+ bool err = false;
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ robot_msgs::PointStamped psi;
+ psi.header = collisionMap->header;
+ psi.point.x = collisionMap->boxes[i].center.x;
+ psi.point.y = collisionMap->boxes[i].center.y;
+ psi.point.z = collisionMap->boxes[i].center.z;
+
+ robot_msgs::PointStamped pso;
+ try
+ {
+ tf_->transformPoint(target, psi, pso);
+ }
+ catch(...)
+ {
+ err = true;
+ pso = psi;
+ }
+
+ poses[i].setRotation(btQuaternion(btVector3(collisionMap->boxes[i].axis.x, collisionMap->boxes[i].axis.y, collisionMap->boxes[i].axis.z), collisionMap->boxes[i].angle));
+ poses[i].setOrigin(btVector3(pso.point.x, pso.point.y, pso.point.z));
+ boxes[i] = new shapes::Box(collisionMap->boxes[i].extents.x + pd, collisionMap->boxes[i].extents.y + pd, collisionMap->boxes[i].extents.z + pd);
+ }
+
+ if (err)
+ ROS_ERROR("Some errors encountered in transforming the collision map to frame '%s' from frame '%s'", target.c_str(), collisionMap->header.frame_id.c_str());
+ }
+ else
+ {
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ poses[i].setRotation(btQuaternion(btVector3(collisionMap->boxes[i].axis.x, collisionMap->boxes[i].axis.y, collisionMap->boxes[i].axis.z), collisionMap->boxes[i].angle));
+ poses[i].setOrigin(btVector3(collisionMap->boxes[i].center.x, collisionMap->boxes[i].center.y, collisionMap->boxes[i].center.z));
+ boxes[i] = new shapes::Box(collisionMap->boxes[i].extents.x + pd, collisionMap->boxes[i].extents.y + pd, collisionMap->boxes[i].extents.z + pd);
+ }
+ }
+}
+
void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
{
boost::mutex::scoped_lock lock(mapUpdateLock_);
@@ -212,7 +270,7 @@
std::vector<shapes::Shape*> spheres;
std::vector<btTransform> poses;
- collisionMapAsSpheres(collisionMap, spheres, poses);
+ collisionMapAsBoxes(collisionMap, spheres, poses);
collisionSpace_->lock();
if (clear)
@@ -307,3 +365,34 @@
return result;
}
+
+void planning_environment::CollisionSpaceMonitor::recoverCollisionMap(const collision_space::EnvironmentModel *env, mapping_msgs::CollisionMap &cmap)
+{
+ cmap.header.frame_id = getFrameId();
+ cmap.header.stamp = ros::Time::now();
+ cmap.boxes.clear();
+
+ const collision_space::EnvironmentObjects::NamespaceObjects &no = env->getObjects()->getObjects("points");
+ const unsigned int n = no.shape.size();
+ double pd = pointcloud_padd_ * 2.0;
+ for (unsigned int i = 0 ; i < n ; ++i)
+ if (no.shape[i]->type == shapes::BOX)
+ {
+ const shapes::Box* box = static_cast<const shapes::Box*>(no.shape[i]);
+ mapping_msgs::OrientedBoundingBox obb;
+ obb.extents.x = box->size[0] - pd;
+ obb.extents.y = box->size[1] - pd;
+ obb.extents.z = box->size[2] - pd;
+ const btVector3 &c = no.shapePose[i].getOrigin();
+ obb.center.x = c.x();
+ obb.center.y = c.y();
+ obb.center.z = c.z();
+ const btQuaternion q = no.shapePose[i].getRotation();
+ obb.angle = q.getAngle();
+ const btVector3 axis = q.getAxis();
+ obb.axis.x = axis.x();
+ obb.axis.y = axis.y();
+ obb.axis.z = axis.z();
+ cmap.boxes.push_back(obb);
+ }
+}
Deleted: pkg/trunk/motion_planning/planning_environment/view_state_validity.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/view_state_validity.launch 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/view_state_validity.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -1,10 +0,0 @@
-<launch>
- <node pkg="planning_environment" type="view_state_validity" respawn="false" output="screen">
- <remap from="collision_map" to="collision_map_occ" />
- <remap from="collision_map_update" to="collision_map_occ_update" />
- <param name="refresh_interval_collision_map" type="double" value="5.0" />
- <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
- <param name="refresh_interval_pose" type="double" value="1.0" />
- <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
- </node>
-</launch>
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -1,8 +1,8 @@
<launch>
<include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+<!-- <include file="$(find pr2_ik)/pr2_ik_rarm_node.launch"/> -->
<include file="$(find pr2_ik)/pr2_ik_rarm_node.launch"/>
- <include file="$(find pr2_ik)/pr2_ik_wc_rarm_node.launch"/>
<rosparam file="$(find pr2_default_controllers)/r_arm_trajectory_controller.yaml" command="load" ns="r_arm_trajectory_controller"/>
Modified: pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml 2009-07-31 04:10:57 UTC (rev 20244)
@@ -28,7 +28,7 @@
l_wrist_flex_link
l_wrist_roll_link
planner_configs:
- RRTkConfig2 pRRTkConfig1 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2l LBKPIECEkConfig2l armIKConfig1
+ RRTkConfig2 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2l LBKPIECEkConfig2l armIKConfig1
right_arm:
links:
@@ -42,7 +42,7 @@
r_wrist_flex_link
r_wrist_roll_link
planner_configs:
- RRTkConfig2 pRRTkConfig1 SBLkConfig2 pSBLkConfig1 KPIECEkConfig1 KPIECEkConfig2r LBKPIECEkConfig2r armIKConfig1
+ RRTkConfig2 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2r LBKPIECEkConfig2r armIKConfig1
arms:
links:
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-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-31 04:10:57 UTC (rev 20244)
@@ -153,15 +153,21 @@
/** \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 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.*/
- virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose) = 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. */
+ /** \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;
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument. */
+ virtual void removeCollidingObjects(const shapes::StaticShape *shape) = 0;
+
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument. */
+ virtual void removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose) = 0;
+
/** \brief Get the objects currently contained in the model */
const EnvironmentObjects* getObjects(void) const;
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-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-31 04:10:57 UTC (rev 20244)
@@ -45,7 +45,7 @@
namespace collision_space
{
- /** \brief A class describing an environment for a kinematic robot using bullet */
+ /** \brief A class describing an environment for a kinematic robot using bullet. This class is still experimental, and methos such as cloning are not implemented. */
class EnvironmentModelBullet : public EnvironmentModel
{
public:
@@ -80,15 +80,21 @@
/** \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 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.*/
- virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose);
+ /** \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. */
+ /** \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);
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument */
+ virtual void removeCollidingObjects(const shapes::StaticShape *shape);
+
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument */
+ virtual void removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose);
+
/** \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
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-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-31 04:10:57 UTC (rev 20244)
@@ -70,15 +70,21 @@
/** \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 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.*/
- virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose);
+ /** \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. */
+ /** \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);
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument */
+ virtual void removeCollidingObjects(const shapes::StaticShape *shape);
+
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument */
+ virtual void removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose);
+
/** \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
@@ -119,6 +125,7 @@
void registerSpace(dSpaceID space);
void registerGeom(dGeomID geom);
+ void unregisterGeom(dGeomID geom);
void clear(void);
void setup(void);
void collide(dGeomID geom, void *data, dNearCallback *nearCallback) const;
@@ -297,6 +304,7 @@
unsigned int max_contacts;
std::vector<EnvironmentModelODE::Contact> *contacts;
std::vector< std::vector<bool> > *selfCollisionTest;
+ dSpaceID selfSpace;
planning_models::KinematicModel::Link *link1;
planning_models::KinematicModel::Link *link2;
@@ -317,7 +325,8 @@
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;
-
+ void removeCollidingObjects(const dGeomID geom);
+
/** \brief Check if thread-specific routines have been called */
void checkThreadInit(void) const;
void freeMemory(void);
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h 2009-07-31 04:10:57 UTC (rev 20244)
@@ -63,13 +63,13 @@
struct NamespaceObjects
{
/** \brief An array of static shapes */
- std::vector< const shapes::StaticShape* > staticShape;
+ std::vector< shapes::StaticShape* > staticShape;
/** \brief An array of shapes */
- std::vector< const shapes::Shape* > shape;
+ std::vector< shapes::Shape* > shape;
/** \brief An array of shape poses */
- std::vector< btTransform > shapePose;
+ std::vector< btTransform > shapePose;
};
/** \brief Get the list of namespaces */
@@ -77,13 +77,22 @@
/** \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, const shapes::StaticShape *shape);
+ 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, const shapes::Shape *shape, const btTransform &pose);
-
+ 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);
Modified: pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-31 04:10:57 UTC (rev 20244)
@@ -292,26 +292,14 @@
return false;
}
-/*
-void collision_space::EnvironmentModelBullet::addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points)
+
+void collision_space::EnvironmentModelBullet::removeCollidingObjects(const shapes::StaticShape *shape)
{
- btTransform t;
- t.setIdentity();
-
- for (unsigned int i = 0 ; i < n ; ++i)
- {
- unsigned int i4 = i * 4;
- btCollisionObject *object = new btCollisionObject();
- btCollisionShape *shape = new btSphereShape(points[i4 + 3]);
- object->setCollisionShape(shape);
- t.setOrigin(btVector3(points[i4], points[i4 + 1], points[i4 + 2]));
- object->setWorldTransform(t);
- m_world->addCollisionObject(object);
- m_obstacles[ns].push_back(object);
- }
}
-*/
+void collision_space::EnvironmentModelBullet::removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose)
+{
+}
void collision_space::EnvironmentModelBullet::addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses)
{
@@ -320,7 +308,7 @@
addObject(ns, shapes[i], poses[i]);
}
-void collision_space::EnvironmentModelBullet::addObject(const std::string &ns, const shapes::StaticShape* shape)
+void collision_space::EnvironmentModelBullet::addObject(const std::string &ns, shapes::StaticShape* shape)
{
btCollisionObject *obj = createCollisionBody(shape);
m_world->addCollisionObject(obj);
@@ -328,7 +316,7 @@
m_objects->addObject(ns, shape);
}
-void collision_space::EnvironmentModelBullet::addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose)
+void collision_space::EnvironmentModelBullet::addObject(const std::string &ns, shapes::Shape *shape, const btTransform &pose)
{
btCollisionObject *obj = createCollisionBody(shape, 1.0, 0.0);
obj->setWorldTransform(pose);
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-31 04:10:57 UTC (rev 20244)
@@ -302,6 +302,59 @@
registerGeom(dSpaceGetGeom(space, i));
}
+void collision_space::EnvironmentModelODE::ODECollide2::unregisterGeom(dGeomID geom)
+{
+ setup();
+
+ Geom g;
+ g.id = geom;
+ dGeomGetAABB(geom, g.aabb);
+
+ Geom *found = NULL;
+
+ std::vector<Geom*>::iterator posStart1 = std::lower_bound(m_geomsX.begin(), m_geomsX.end(), &g, SortByXTest());
+ std::vector<Geom*>::iterator posEnd1 = std::upper_bound(posStart1, m_geomsX.end(), &g, SortByXTest());
+ while (posStart1 < posEnd1)
+ {
+ if ((*posStart1)->id == geom)
+ {
+ found = *posStart1;
+ m_geomsX.erase(posStart1);
+ break;
+ }
+ ++posStart1;
+ }
+
+ std::vector<Geom*>::iterator posStart2 = std::lower_bound(m_geomsY.begin(), m_geomsY.end(), &g, SortByYTest());
+ std::vector<Geom*>::iterator posEnd2 = std::upper_bound(posStart2, m_geomsY.end(), &g, SortByYTest());
+ while (posStart2 < posEnd2)
+ {
+ if ((*posStart2)->id == geom)
+ {
+ assert(found == *posStart2);
+ m_geomsY.erase(posStart2);
+ break;
+ }
+ ++posStart2;
+ }
+
+ std::vector<Geom*>::iterator posStart3 = std::lower_bound(m_geomsZ.begin(), m_geomsZ.end(), &g, SortByZTest());
+ std::vector<Geom*>::iterator posEnd3 = std::upper_bound(posStart3, m_geomsZ.end(), &g, SortByZTest());
+ while (posStart3 < posEnd3)
+ {
+ if ((*posStart3)->id == geom)
+ {
+ assert(found == *posStart3);
+ m_geomsZ.erase(posStart3);
+ break;
+ }
+ ++posStart3;
+ }
+
+ assert(found);
+ delete found;
+}
+
void collision_space::EnvironmentModelODE::ODECollide2::registerGeom(dGeomID geom)
{
Geom *g = new Geom();
@@ -431,16 +484,21 @@
if (cdata->selfCollisionTest)
{
- EnvironmentModelODE::kGeom* kg1 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o1));
- EnvironmentModelODE::kGeom* kg2 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o2));
- if (kg1 && kg2)
+ dSpaceID s1 = dGeomGetSpace(o1);
+ dSpaceID s2 = dGeomGetSpace(o2);
+ if (s1 == s2 && s1 == cdata->selfSpace)
{
- if (cdata->selfCollisionTest->at(kg1->index)[kg2->index] == false)
- return;
- else
+ EnvironmentModelODE::kGeom* kg1 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o1));
+ EnvironmentModelODE::kGeom* kg2 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o2));
+ if (kg1 && kg2)
{
- cdata->link1 = kg1->link;
- cdata->link2 = kg2->link;
+ if (cdata->selfCollisionTest->at(kg1->index)[kg2->index] == false)
+ return;
+ else
+ {
+ cdata->link1 = kg1->link;
+ cdata->link2 = kg2->link;
+ }
}
}
}
@@ -500,7 +558,6 @@
bool collision_space::EnvironmentModelODE::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count)
{
CollisionData cdata;
- cdata.selfCollisionTest = &m_selfCollisionTest;
cdata.contacts = &contacts;
cdata.max_contacts = max_count;
contacts.clear();
@@ -512,7 +569,6 @@
bool collision_space::EnvironmentModelODE::isCollision(void)
{
CollisionData cdata;
- cdata.selfCollisionTest = &m_selfCollisionTest;
checkThreadInit();
testCollision(&cdata);
return cdata.collides;
@@ -521,14 +577,15 @@
bool collision_space::EnvironmentModelODE::isSelfCollision(void)
{
CollisionData cdata;
- cdata.selfCollisionTest = &m_selfCollisionTest;
checkThreadInit();
testSelfCollision(&cdata);
return cdata.collides;
}
void collision_space::EnvironmentModelODE::testSelfCollision(CollisionData *cdata)
-{
+{
+ cdata->selfCollisionTest = &m_selfCollisionTest;
+ cdata->selfSpace = m_modelGeom.space;
dSpaceCollide(m_modelGeom.space, cdata, nearCallbackFn);
}
@@ -622,6 +679,7 @@
{
dGeomID g = createODEGeom(cn->space, cn->storage, shapes[i], 1.0, 0.0);
assert(g);
+ dGeomSetData(g, reinterpret_cast<void*>(shapes[i]));
updateGeom(g, poses[i]);
cn->collide2.registerGeom(g);
m_objects->addObject(ns, shapes[i], poses[i]);
@@ -629,7 +687,7 @@
cn->collide2.setup();
}
-void collision_space::EnvironmentModelODE::addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose)
+void collision_space::EnvironmentModelODE::addObject(const std::string &ns, shapes::Shape *shape, const btTransform &pose)
{
std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.find(ns);
CollisionNamespace* cn = NULL;
@@ -643,12 +701,13 @@
dGeomID g = createODEGeom(cn->space, cn->storage, shape, 1.0, 0.0);
assert(g);
+ dGeomSetData(g, reinterpret_cast<void*>(shape));
updateGeom(g, pose);
cn->geoms.push_back(g);
m_objects->addObject(ns, shape, pose);
}
-void collision_space::EnvironmentModelODE::addObject(const std::string &ns, const shapes::StaticShape* shape)
+void collision_space::EnvironmentModelODE::addObject(const std::string &ns, shapes::StaticShape* shape)
{
std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.find(ns);
CollisionNamespace* cn = NULL;
@@ -662,6 +721,7 @@
dGeomID g = createODEGeom(cn->space, cn->storage, shape);
assert(g);
+ dGeomSetData(g, reinterpret_cast<void*>(shape));
cn->geoms.push_back(g);
m_objects->addObject(ns, shape);
}
@@ -682,6 +742,103 @@
m_objects->clearObjects(ns);
}
+void collision_space::EnvironmentModelODE::removeCollidingObjects(const shapes::StaticShape *shape)
+{
+ checkThreadInit();
+ dSpaceID space = dSimpleSpaceCreate(0);
+ ODEStorage storage;
+ dGeomID g = createODEGeom(space, storage, shape);
+ removeCollidingObjects(g);
+ dSpaceDestroy(space);
+}
+
+void collision_space::EnvironmentModelODE::removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose)
+{
+ checkThreadInit();
+ dSpaceID space = dSimpleSpaceCreate(0);
+ ODEStorage storage;
+ dGeomID g = createODEGeom(space, storage, shape, 1.0, 0.0);
+ updateGeom(g, pose);
+ removeCollidingObjects(g);
+ dSpaceDestroy(space);
+}
+
+void collision_space::EnvironmentModelODE::removeCollidingObjects(dGeomID geom)
+{
+ CollisionData cdata;
+ for (std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.begin() ; it != m_collNs.end() ; ++it)
+ {
+ std::map<void*, bool> remove;
+
+ // update the set of geoms
+ unsigned int n = it->second->geoms.size();
+ std::vector<dGeomID> replaceGeoms;
+ replaceGeoms.reserve(n);
+ for (unsigned int j = 0 ; j < n ; ++j)
+ {
+ cdata.done = cdata.collides = false;
+ dSpaceCollide2(geom, it->second->geoms[j], &cdata, nearCallbackFn);
+ if (cdata.collides)
+ {
+ remove[dGeomGetData(it->second->geoms[j])] = true;
+ dGeomDestroy(it->second->geoms[j]);
+ }
+ else
+ {
+ replaceGeoms.push_back(it->second->geoms[j]);
+ remove[dGeomGetData(it->second->geoms[j])] = false;
+ }
+ }
+ it->second->geoms = replaceGeoms;
+
+ // update the collide2 structure
+ std::vector<dGeomID> geoms;
+ it->second->collide2.getGeoms(geoms);
+ n = geoms.size();
+ for (unsigned int j = 0 ; j < n ; ++j)
+ {
+ cdata.done = cdata.collides = false;
+ dSpaceCollide2(geom, geoms[j], &cdata, nearCallbackFn);
+ if (cdata.collides)
+ {
+ remove[dGeomGetData(geoms[j])] = true;
+ it->second->collide2.unregisterGeom(geoms[j]);
+ dGeomDestroy(geoms[j]);
+ }
+ else
+ remove[dGeomGetData(geoms[j])] = false;
+ }
+
+ EnvironmentObjects::NamespaceObjects &no = m_objects->getObjects(it->first);
+
+ std::vector<shapes::Shape*> replaceShapes;
+ std::vector<btTransform> replaceShapePoses;
+ n = no.shape.size();
+ replaceShapes.reserve(n);
+ replaceShapePoses.reserve(n);
+ for (unsigned int i = 0 ; i < n ; ++i)
+ if (remove[reinterpret_cast<void*>(no.shape[i])])
+ delete no.shape[i];
+ else
+ {
+ replaceShapes.push_back(no.shape[i]);
+ replaceShapePoses.push_back(no.shapePose[i]);
+ }
+ no.shape = replaceShapes;
+ no.shapePose = replaceShapePoses;
+
+ std::vector<shapes::StaticShape*> replaceStaticShapes;
+ n = no.staticShape.size();
+ replaceStaticShapes.resize(n);
+ for (unsigned int i = 0 ; i < n ; ++i)
+ if (remove[reinterpret_cast<void*>(no.staticShape[i])])
+ delete no.staticShape[i];
+ else
+ replaceStaticShapes.push_back(no.staticShape[i]);
+ no.staticShape = replaceStaticShapes;
+ }
+}
+
int collision_space::EnvironmentModelODE::setCollisionCheck(const std::string &link, bool state)
{
int result = -1;
@@ -785,22 +942,64 @@
env->createODERobotModel();
for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
env->m_modelGeom.linkGeom[j]->enabled = m_modelGeom.linkGeom[j]->enabled;
+
for (std::map<std::string, CollisionNamespace*>::const_iterator it = m_collNs.begin() ; it != m_collNs.end() ; ++it)
{
+ // construct a map of the shape pointers we have; this points to the index positions where they are stored;
+ std::map<void*, int> shapePtrs;
+ const EnvironmentObjects::NamespaceObjects &ns = m_objects->getObjects(it->first);
+ unsigned int n = ns.staticShape.size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ shapePtrs[ns.staticShape[i]] = -1 - i;
+ n = ns.shape.size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ shapePtrs[ns.shape[i]] = i;
+
+ // copy the collision namespace structure, geom by geom
CollisionNamespace *cn = new CollisionNamespace(it->first);
env->m_collNs[it->first] = cn;
- unsigned int n = it->second->geoms.size();
+ n = it->second->geoms.size();
cn->geoms.reserve(n);
for (unsigned int i = 0 ; i < n ; ++i)
- cn->geoms.push_back(copyGeom(cn->space, cn->storage, it->second->geoms[i], it->second->storage));
+ {
+ dGeomID newGeom = copyGeom(cn->space, cn->storage, it->second->geoms[i], it->second->storage);
+ int idx = shapePtrs[dGeomGetData(it->second->geoms[i])];
+ if (idx < 0) // static geom
+ {
+ shapes::StaticShape *newShape = shapes::clone_shape(ns.staticShape[-idx - 1]);
+ dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
+ env->m_objects->addObject(it->first, newShape);
+ }
+ else // movable geom
+ {
+ shapes::Shape *newShape = shapes::clone_shape(ns.shape[idx]);
+ dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
+ env->m_objects->addObject(it->first, newShape, ns.shapePose[idx]);
+ }
+ cn->geoms.push_back(newGeom);
+ }
std::vector<dGeomID> geoms;
it->second->collide2.getGeoms(geoms);
n = geoms.size();
for (unsigned int i = 0 ; i < n ; ++i)
- cn->collide2.registerGeom(copyGeom(cn->space, cn->storage, geoms[i], it->second->storage));
+ {
+ dGeomID newGeom = copyGeom(cn->space, cn->storage, geoms[i], it->second->storage);
+ int idx = shapePtrs[dGeomGetData(geoms[i])];
+ if (idx < 0) // static geom
+ {
+ shapes::StaticShape *newShape = shapes::clone_shape(ns.staticShape[-idx - 1]);
+ dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
+ env->m_objects->addObject(it->first, newShape);
+ }
+ else // movable geom
+ {
+ shapes::Shape *newShape = shapes::clone_shape(ns.shape[idx]);
+ dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
+ env->m_objects->addObject(it->first, newShape, ns.shapePose[idx]);
+ }
+ cn->collide2.registerGeom(newGeom);
+ }
}
- if (env->m_objects)
- delete env->m_objects;
- env->m_objects = m_objects->clone();
+
return env;
}
Modified: pkg/trunk/world_models/collision_space/src/environment_objects.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment_objects.cpp 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/src/environment_objects.cpp 2009-07-31 04:10:57 UTC (rev 20244)
@@ -53,17 +53,55 @@
return it->second;
}
-void collision_space::EnvironmentObjects::addObject(const std::string &ns, const shapes::StaticShape *shape)
+collision_space::EnvironmentObjects::NamespaceObjects& collision_space::EnvironmentObjects::getObjects(const std::string &ns)
{
+ return m_objects[ns];
+}
+
+void collision_space::EnvironmentObjects::addObject(const std::string &ns, 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)
+void collision_space::EnvironmentObjects::addObject(const std::string &ns, shapes::Shape *shape, const btTransform &pose)
{
m_objects[ns].shape.push_back(shape);
m_objects[ns].shapePose.push_back(pose);
}
+bool collision_space::EnvironmentObjects::removeObject(const std::string &ns, const shapes::Shape *shape)
+{
+ std::map<std::string, NamespaceObjects>::iterator it = m_objects.find(ns);
+ if (it != m_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.shapePose.erase(it->second.shapePose.begin() + i);
+ return true;
+ }
+ }
+ return false;
+}
+
+bool collision_space::EnvironmentObjects::removeObject(const std::string &ns, const shapes::StaticShape *shape)
+{
+ 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)
+ if (it->second.staticShape[i] == shape)
+ {
+ it->second.staticShape.erase(it->second.staticShape.begin() + i);
+ return true;
+ }
+ }
+ return false;
+}
+
void collision_space::EnvironmentObjects::clearObjects(const std::string &ns)
{
std::map<std::string, NamespaceObjects>::iterator it = m_objects.find(ns);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
