Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7313] pkg/trunk
|
From: <rdi...@us...> - 2008-11-26 00:00:35
|
Revision: 7313
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7313&view=rev
Author: rdiankov
Date: 2008-11-26 00:00:29 +0000 (Wed, 26 Nov 2008)
Log Message:
-----------
or_collision example is usable as a command-line tool now
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/or_collision/CMakeLists.txt
pkg/trunk/openrave_planning/or_collision/manifest.xml
Added Paths:
-----------
pkg/trunk/openrave_planning/or_collision/or_collision.cpp
Removed Paths:
-------------
pkg/trunk/openrave_planning/or_collision/orcollision.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-11-25 23:18:26 UTC (rev 7312)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-11-26 00:00:29 UTC (rev 7313)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 509
+SVN_REVISION = -r 513
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2008-11-25 23:18:26 UTC (rev 7312)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2008-11-26 00:00:29 UTC (rev 7313)
@@ -29,9 +29,6 @@
#include "ros/node.h"
#include "rosthread/member_thread.h"
-#includle "std_srvs/StaticMap.h"
-#includle "std_srvs/PolledImage.h"
-
using namespace std;
using namespace ros;
using namespace ros::thread;
@@ -72,21 +69,17 @@
return pnode;
}
- void startsession()
+ void advertise_session()
{
ros::node* pnode = startros();
if( pnode == NULL )
return;
- vector<string> vnames;
- vnames.push_back("orgetmap");
- vnames.push_back("orgetimage");
- sessionhandle = pnode->advertise_session("mysession"
+ sessionhandle = pnode->advertise_service("openrave_service",&OpenraveSession::startsession,this);
}
- bool terminate(int id) {
- cout << "terminate session: " << id << endl;
- }
+ bool startsession(roscpp_tutorials::simple_session::request& req, roscpp_tutorials::simple_session::response& res) {
+ if( req.sessionid ) {
bool getmap(StaticMap::request& req, StaticMap::response& res)
{
Modified: pkg/trunk/openrave_planning/or_collision/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/or_collision/CMakeLists.txt 2008-11-25 23:18:26 UTC (rev 7312)
+++ pkg/trunk/openrave_planning/or_collision/CMakeLists.txt 2008-11-26 00:00:29 UTC (rev 7313)
@@ -2,5 +2,5 @@
include(rosbuild)
add_definitions(-Wall)
rospack(or_collision)
-add_executable(or_collision orcollision.cpp)
-target_link_libraries (or_collision pthread openrave-core)
+add_executable(or_collision or_collision.cpp)
+target_link_libraries (or_collision openrave-core)
Modified: pkg/trunk/openrave_planning/or_collision/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/or_collision/manifest.xml 2008-11-25 23:18:26 UTC (rev 7312)
+++ pkg/trunk/openrave_planning/or_collision/manifest.xml 2008-11-26 00:00:29 UTC (rev 7313)
@@ -1,11 +1,9 @@
<package>
- <description brief="OpenRAVE Session for ROS">
- Main openrave server that provides a session interface.
+ <description brief="OpenRAVE Collision Checking">
+ Small test program that loads up the OpenRAVE run-time engine, loads a robot model, and checks for self-collisions. The program will return the contact points of the self-collisions. Various openrave checkers can be set to be tested.
</description>
<author>Rosen Diankov (rdi...@cs...)</author>
<license>BSD</license>
<depend package="roscpp"/>
<depend package="openrave"/>
- <sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
</package>
Copied: pkg/trunk/openrave_planning/or_collision/or_collision.cpp (from rev 7284, pkg/trunk/openrave_planning/or_collision/orcollision.cpp)
===================================================================
--- pkg/trunk/openrave_planning/or_collision/or_collision.cpp (rev 0)
+++ pkg/trunk/openrave_planning/or_collision/or_collision.cpp 2008-11-26 00:00:29 UTC (rev 7313)
@@ -0,0 +1,173 @@
+#include <openrave-core.h>
+#include <vector>
+#include <cstring>
+#include <sstream>
+
+using namespace OpenRAVE;
+using namespace std;
+
+void printhelp();
+void printinterfaces(EnvironmentBase*);
+
+int main(int argc, char ** argv)
+{
+ if( argc < 2 ) {
+ printhelp();
+ return -1; // no robots to load
+ }
+
+ // create the main environment
+ EnvironmentBase* penv = CreateEnvironment();
+ vector<dReal> vsetvalues;
+
+ // parse the command line options
+ int i = 1;
+ while(i < argc) {
+ if( strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "-?") == 0 || strcmp(argv[i], "/?") == 0 || strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 ) {
+ printhelp();
+ return 0;
+ }
+ else if( strcmp(argv[i], "--checker") == 0 ) {
+ // create requested collision checker
+ CollisionCheckerBase* pchecker = penv->CreateCollisionChecker(argv[i+1]);
+ if( pchecker == NULL ) {
+ RAVEPRINT(L"failed to create checker %s\n", argv[i+1]);
+ return -3;
+ }
+ penv->SetCollisionChecker(pchecker);
+ i += 2;
+ }
+ else if( strcmp(argv[i], "--list") == 0 ) {
+ printinterfaces(penv);
+ return 0;
+ }
+ else if( strcmp(argv[i], "--joints") == 0 ) {
+ vsetvalues.resize(atoi(argv[i+1]));
+ for(int j = 0; j < (int)vsetvalues.size(); ++j)
+ vsetvalues[j] = atoi(argv[i+j+2]);
+
+ i += 2+vsetvalues.size();
+ }
+ else
+ break;
+ }
+
+
+ if( i >= argc ) {
+ RAVEPRINT(L"not enough parameters\n");
+ printhelp();
+ return -1;
+ }
+
+ // load the scene
+ if( !penv->Load(argv[i]) ) {
+ printhelp();
+ return -2;
+ }
+
+ // get the first robot
+ if( penv->GetRobots().size() == 0 )
+ return -3;
+
+ RobotBase* probot = penv->GetRobots().front();
+
+ vector<dReal> values; probot->GetJointValues(values);
+
+ // set new values
+ for(int i = 0; i < (int)vsetvalues.size() && i < (int)values.size(); ++i)
+ values[i] = vsetvalues[i];
+
+ probot->SetJointValues(NULL,NULL,&values[0],true);
+
+ int contactpoints = 0;
+ COLLISIONREPORT report;
+ penv->SetCollisionOptions(CO_Contacts);
+ if( probot->CheckSelfCollision(&report) ) {
+ contactpoints = (int)report.contacts.size();
+ wstringstream ss;
+ ss << "robot in self-collision "
+ << (report.plink1 != NULL ? report.plink1->GetName() : L"") << ":"
+ << (report.plink2 != NULL ? report.plink2->GetName() : L"") << " at "
+ << contactpoints << "contacts" << endl;
+ for(int i = 0; i < contactpoints; ++i) {
+ COLLISIONREPORT::CONTACT& c = report.contacts[i];
+ ss << "contact" << i << ": pos=("
+ << c.pos.x << ", " << c.pos.y << ", " << c.pos.z << "), norm=("
+ << c.norm.x << ", " << c.norm.y << ", " << c.norm.z << ")" << endl;
+ }
+
+ RAVEPRINT(ss.str().c_str());
+ }
+ else RAVEPRINT(L"robot not in collision\n");
+
+ // get the transformations of all the links
+ vector<Transform> vlinktransforms;
+ probot->GetBodyTransformations(vlinktransforms);
+
+ return contactpoints;
+}
+
+void printhelp()
+{
+ printf("or_collision [--list] [--checker checker_name] [--joints #values [values]] robot_model\n"
+ " Load a robot into the openrave environment, set it at [joint values] and\n"
+ " check for self collisions. Returns number of contact points.\n"
+ "-listplugins Will list all the loadable interfaces (ie, collision checkers).\n"
+ "-checker name Load a different collision checker instead of the default one\n"
+ "-joints #values [values] Set the robot to specific joint values\n");
+}
+
+void printinterfaces(EnvironmentBase* penv)
+{
+ PLUGININFO info;
+ penv->GetLoadedInterfaces(info);
+
+ vector<wstring>::const_iterator itnames;
+ vector<string> names;
+ vector<string>::iterator itname;
+ wstringstream ss;
+
+ ss << endl << L"Loadable interfaces: " << endl;
+
+ ss << L"Collision Checkers (" << info.collisioncheckers.size() << "):" << endl;
+ for(itnames = info.collisioncheckers.begin(); itnames != info.collisioncheckers.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Controllers (" << info.controllers.size() << "):" << endl;
+ for(itnames = info.controllers.begin(); itnames != info.controllers.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Inverse Kinematics Solvers (" << info.iksolvers.size() << "):" << endl;
+ for(itnames = info.iksolvers.begin(); itnames != info.iksolvers.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Physics Engines (" << info.physicsengines.size() << "):" << endl;
+ for(itnames = info.physicsengines.begin(); itnames != info.physicsengines.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Planners (" << info.planners.size() << "):" << endl;
+ for(itnames = info.planners.begin(); itnames != info.planners.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Problems (" << info.problems.size() << "):" << endl;
+ for(itnames = info.problems.begin(); itnames != info.problems.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Robots (" << info.robots.size() << "):" << endl;
+ for(itnames = info.robots.begin(); itnames != info.robots.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Sensors (" << info.sensors.size() << "):" << endl;
+ for(itnames = info.sensors.begin(); itnames != info.sensors.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Sensor Systems (" << info.sensorsystems.size() << "):" << endl;
+ for(itnames = info.sensorsystems.begin(); itnames != info.sensorsystems.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Trajectories (" << info.trajectories.size() << "):" << endl;
+ for(itnames = info.trajectories.begin(); itnames != info.trajectories.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ RAVEPRINT(ss.str().c_str());
+}
Deleted: pkg/trunk/openrave_planning/or_collision/orcollision.cpp
===================================================================
--- pkg/trunk/openrave_planning/or_collision/orcollision.cpp 2008-11-25 23:18:26 UTC (rev 7312)
+++ pkg/trunk/openrave_planning/or_collision/orcollision.cpp 2008-11-26 00:00:29 UTC (rev 7313)
@@ -1,50 +0,0 @@
-#include <openrave-core.h>
-#include <vector>
-using namespace OpenRAVE;
-using namespace std;
-
-int main(int argc, char ** argv)
-{
- if( argc < 2 )
- return -1; // no robots to load
-
- EnvironmentBase* penv = CreateEnvironment();
- // load the scene
- if( !penv->Load(argv[1]) )
- return -2;
-
- // choose collision checker
- if( argc > 2 ) {
- CollisionCheckerBase* pchecker = penv->CreateCollisionChecker(argv[2]);
- if( pchecker == NULL ) {
- RAVEPRINT(L"failed to create checker %s\n", argv[2]);
- return -3;
- }
- penv->SetCollisionChecker(pchecker);
- }
-
- // get the first robot
- if( penv->GetRobots().size() == 0 )
- return -3;
-
- RobotBase* probot = penv->GetRobots().front();
-
- vector<dReal> values;
- probot->GetJointValues(values);
-
- values[1] = 1;
- probot->SetJointValues(NULL,NULL,&values[0]);
-
- COLLISIONREPORT report;
- if( probot->CheckSelfCollision(&report) ) {
- RAVEPRINT(L"robot in self-collision %S:%S at %d contacts\n",
- report.plink1 != NULL ? report.plink1->GetName() : L"",
- report.plink2 != NULL ? report.plink2->GetName() : L"", report.contacts.size());
- }
-
- // get the transformations of all the links
- vector<Transform> vlinktransforms;
- probot->GetBodyTransformations(vlinktransforms);
-
- return 0;
-}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
