Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9200] pkg/trunk
|
From: <rdi...@us...> - 2009-01-11 10:18:14
|
Revision: 9200
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9200&view=rev
Author: rdiankov
Date: 2009-01-11 10:18:03 +0000 (Sun, 11 Jan 2009)
Log Message:
-----------
new openrave sensor system for displaying a collision map
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/mapping/collision_map/manifest.xml
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
pkg/trunk/util/laser_scan/CMakeLists.txt
pkg/trunk/util/laser_scan/mainpage.dox
pkg/trunk/util/robot_self_filter/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-11 10:18:03 UTC (rev 9200)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 602
+SVN_REVISION = -r 603
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-01-11 10:18:03 UTC (rev 9200)
@@ -12,7 +12,10 @@
<depend package="std_msgs" />
<depend package="cloud_geometry" />
<depend package="tf" />
-
+ <export>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
+ </export>
+
<sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="liblapack-dev"/>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-11 10:18:03 UTC (rev 9200)
@@ -21,3 +21,8 @@
if( isempty(out) )
error('failed to create phasespace');
end
+
+out = orProblemSendCommand('createsystem CollisionMap collision_map',probs.task);
+if( isempty(out) )
+ error('failed to create phasespace');
+end
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-11 10:18:03 UTC (rev 9200)
@@ -14,4 +14,5 @@
<depend package="robot_msgs"/>
<depend package="pr2_mechanism_controllers"/>
<depend package="tf"/>
+ <depend package="collision_map"/>
</package>
Added: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-11 10:18:03 UTC (rev 9200)
@@ -0,0 +1,118 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Rosen Diankov
+// 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.
+// * The name of the author may not 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 Rosen Diankov
+#ifndef COLLISIONMAP_MOCAP_SYSTEM
+#define COLLISIONMAP_MOCAP_SYSTEM
+
+#include "rossensorsystem.h"
+#include <collision_map/CollisionMap.h>
+
+// used to update objects through a mocap system
+class CollisionMapXMLID
+{
+public:
+ static const char* GetXMLId() { return "collisionmap"; }
+};
+
+class CollisionMapSystem : public ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>
+{
+public:
+ CollisionMapSystem(EnvironmentBase* penv)
+ : ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>(penv), _nNextId(1)
+ {
+ }
+
+ virtual bool Init(istream& sinput)
+ {
+ _topic = "collision_map";
+ return ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::Init(sinput);
+ }
+
+private:
+ void newdatacb()
+ {
+ // create the new kinbody
+ KinBody* pbody = GetEnv()->CreateKinBody();
+
+ _vaabbs.resize(_topicmsg.boxes.size());
+ vector<AABB>::iterator itab = _vaabbs.begin();
+ FOREACH(itmsgab, _topicmsg.boxes) {
+ itab->pos = Vector(itmsgab->center.x, itmsgab->center.y, itmsgab->center.z);
+ itab->extents = Vector(itmsgab->extents.x, itmsgab->extents.y, itmsgab->extents.z);
+ ++itab;
+ }
+
+ if( !pbody->InitFromBoxes(_vaabbs, true) ) {
+ RAVELOG_ERRORA("failed to create collision map\n");
+ delete pbody;
+ return;
+ }
+
+ // append an id to the body
+ stringstream ss;
+ ss << "CollisionMap" << _nNextId++;
+ pbody->SetName(ss.str().c_str());
+
+ // add the new kinbody
+ if( !GetEnv()->AddKinBody(pbody) ) {
+ RAVELOG_ERRORA("failed to add body %S\n", pbody->GetName());
+ delete pbody;
+ return;
+ }
+
+ {
+ boost::mutex::scoped_lock lock(_mutex);
+ GetEnv()->LockPhysics(true);
+
+ // remove all unlocked bodies
+ TYPEOF(_mapbodies.begin()) itbody = _mapbodies.begin();
+ while(itbody != _mapbodies.end()) {
+ if( !itbody->second->IsLocked() ) {
+ _mapbodies.erase(itbody++);
+ }
+ else
+ ++itbody;
+ }
+
+ GetEnv()->LockPhysics(false);
+ }
+
+ BODY* b = AddKinBody(pbody, NULL);
+ if( b == NULL ) {
+ RAVELOG_ERRORA("removing/destroying kinbody\n");
+ GetEnv()->RemoveKinBody(pbody, true);
+ }
+ }
+
+ Transform GetTransform(const std_msgs::Transform& pose)
+ {
+ return Transform(Vector(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z), Vector(pose.translation.x, pose.translation.y, pose.translation.z));
+ }
+
+ vector<AABB> _vaabbs;
+ int _nNextId;
+};
+
+#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-01-11 10:18:03 UTC (rev 9200)
@@ -22,7 +22,7 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
-// author: Rosen Diankov
+// \author Rosen Diankov
#ifndef OBJECTTRANSFORM_SENSOR_SYSTEM
#define OBJECTTRANSFORM_SENSOR_SYSTEM
@@ -42,7 +42,7 @@
{
public:
ObjectTransformSystem(EnvironmentBase* penv)
- : ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>(penv), _robotid(0), nNextId(1)
+ : ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>(penv), _robotid(0), _nNextId(1)
{
}
virtual ~ObjectTransformSystem() {
@@ -227,7 +227,7 @@
// append an id to the body
wstringstream ss;
- ss << pbody->GetName() << nNextId++;
+ ss << pbody->GetName() << _nNextId++;
pbody->SetName(ss.str().c_str());
if( !GetEnv()->AddKinBody(pbody) ) {
@@ -238,7 +238,7 @@
BODY* b = AddKinBody(pbody, NULL);
if( b == NULL ) {
- delete pbody;
+ GetEnv()->RemoveKinBody(pbody, true);
continue;
}
@@ -272,7 +272,7 @@
int _robotid;
Transform _toffset; ///< offset from tf frame
dReal _fThreshSqr;
- int nNextId;
+ int _nNextId;
};
#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-01-11 10:18:03 UTC (rev 9200)
@@ -22,12 +22,13 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
-// author: Rosen Diankov
+// \author Rosen Diankov
#include "plugindefs.h"
#include "rosarmik.h"
#include "phasespacesystem.h"
#include "objecttransformsystem.h"
+#include "collisionmapsystem.h"
#include "rosrobotcontroller.h"
#include "rosplanningproblem.h"
@@ -68,8 +69,10 @@
case PT_SensorSystem:
if( wcsicmp(name, L"PhaseSpace") == 0 )
return new PhaseSpaceMocapClient(penv);
- if( wcsicmp(name, L"ObjectTransform") == 0 )
+ else if( wcsicmp(name, L"ObjectTransform") == 0 )
return new ObjectTransformSystem(penv);
+ else if( wcsicmp(name, L"CollisionMap") == 0 )
+ return new CollisionMapSystem(penv);
break;
default:
break;
@@ -86,9 +89,11 @@
return false;
}
+ pinfo->controllers.push_back(L"ROSRobot");
pinfo->iksolvers.push_back(L"ROSArmIK");
pinfo->sensorsystems.push_back(L"PhaseSpace");
pinfo->sensorsystems.push_back(L"ObjectTransform");
+ pinfo->sensorsystems.push_back(L"CollisionMap");
pinfo->problems.push_back(L"ROSPlanning");
return true;
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2009-01-11 10:18:03 UTC (rev 9200)
@@ -22,7 +22,7 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
-// author: Rosen Diankov
+// \author Rosen Diankov
#ifndef ROS_SENSORSYSTEM_SYSTEM
#define ROS_SENSORSYSTEM_SYSTEM
Modified: pkg/trunk/util/laser_scan/CMakeLists.txt
===================================================================
--- pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-11 10:18:03 UTC (rev 9200)
@@ -1,5 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
+set(ROS_BUILD_TYPE RelWithDebInfo)
rospack(laser_scan)
rospack_add_library(laser_scan src/laser_scan.cc )
rospack_add_library(laser_median_filter src/median_filter.cpp )
Modified: pkg/trunk/util/laser_scan/mainpage.dox
===================================================================
--- pkg/trunk/util/laser_scan/mainpage.dox 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/util/laser_scan/mainpage.dox 2009-01-11 10:18:03 UTC (rev 9200)
@@ -21,10 +21,6 @@
\subsection mean Mean Filter
\todo Document
-\subsection robotlinks_filter Robot Links Pruning Filter
-
-RobotLinksFilter removes all nodes that lie on the robot. Each robot link is approximated by a convex hull that is dynamically generated based on the mesh of the robot.
-
\subsection shadows Scan Shadows Filter
\todo document
Modified: pkg/trunk/util/robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/robot_self_filter/CMakeLists.txt 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/util/robot_self_filter/CMakeLists.txt 2009-01-11 10:18:03 UTC (rev 9200)
@@ -1,6 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
-set(ROS_BUILD_TYPE RelDebInfo)
+set(ROS_BUILD_TYPE RelWithDebInfo)
rospack(robot_self_filter)
rospack_add_executable(robotlinks_filter_node src/robotlinks_filter_node.cpp)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
