Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8479] pkg/trunk
|
From: <rdi...@us...> - 2008-12-20 03:33:35
|
Revision: 8479
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8479&view=rev
Author: rdiankov
Date: 2008-12-20 03:33:29 +0000 (Sat, 20 Dec 2008)
Log Message:
-----------
added a checkerboard detector that can be configured to detect any number of checkerboards, added an openrave sensor system ObjectTransform to connect to the ROS network and get the detection data, added openrave scripts to connect to the camera and manage the detected boxes in the openrave environment
Modified Paths:
--------------
pkg/trunk/openrave_planning/openraveros/manifest.xml
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/ormanipulation/data/ricebox.kinbody.xml
pkg/trunk/openrave_planning/ormanipulation/octave/testscene.m
pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
Added Paths:
-----------
pkg/trunk/openrave_planning/ormanipulation/startcameras.ros.xml
pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
pkg/trunk/vision/checkerboard_detector/
pkg/trunk/vision/checkerboard_detector/CMakeLists.txt
pkg/trunk/vision/checkerboard_detector/Makefile
pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
pkg/trunk/vision/checkerboard_detector/manifest.xml
pkg/trunk/vision/checkerboard_detector/math.h
pkg/trunk/vision/checkerboard_detector/msg/
pkg/trunk/vision/checkerboard_detector/msg/Object6DPose.msg
pkg/trunk/vision/checkerboard_detector/msg/ObjectDetection.msg
Modified: pkg/trunk/openrave_planning/openraveros/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/openraveros/manifest.xml 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/openraveros/manifest.xml 2008-12-20 03:33:29 UTC (rev 8479)
@@ -10,4 +10,5 @@
<depend package="openrave"/>
<depend package="std_msgs"/>
<depend package="rosoct"/>
+ <url>http://pr.willowgarage.com/wiki/openrave</url>
</package>
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-20 03:33:29 UTC (rev 8479)
@@ -544,14 +544,18 @@
KinBody* pbody = GetEnv()->CreateKinBody();
if( req.file.size() > 0 ) {
- if( !pbody->Init(req.file.c_str(), NULL) )
+ if( !pbody->Init(req.file.c_str(), NULL) ) {
+ delete pbody;
return false;
+ }
}
pbody->SetName(req.name.c_str());
- if( !GetEnv()->AddKinBody(pbody) )
+ if( !GetEnv()->AddKinBody(pbody) ) {
+ delete pbody;
return false;
+ }
res.bodyid = pbody->GetNetworkId();
return true;
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-20 03:33:29 UTC (rev 8479)
@@ -36,7 +36,7 @@
{ \
SessionState state = getstate(req); /* need separate copy in order to guarantee thread safety */ \
if( !state._pserver ) { \
- RAVELOG_INFOA("failed to find session for service %s", #srvname); \
+ RAVELOG_INFOA("failed to find session for service %s\n", #srvname); \
return false; \
} \
return state._pserver->srvname##_srv(req,res); \
@@ -330,7 +330,7 @@
boost::mutex::scoped_lock lock(_mutexsession);
if( _mapsessions.find(req.sessionid) != _mapsessions.end() ) {
_mapsessions.erase(req.sessionid);
- RAVELOG_INFOA("destroyed openrave session: %d", req.sessionid);
+ RAVELOG_INFOA("destroyed openrave session: %d\n", req.sessionid);
return true;
}
@@ -352,14 +352,14 @@
}
if( !clonestate._penv )
- RAVELOG_INFOA("failed to find session %d", req.clone_sessionid);
+ RAVELOG_INFOA("failed to find session %d\n", req.clone_sessionid);
else
state._penv.reset(clonestate._penv->CloneSelf(req.clone_options));
}
if( !state._penv ) {
// cloning from parent
- RAVELOG_DEBUGA("cloning from parent");
+ RAVELOG_DEBUGA("cloning from parent\n");
state._penv.reset(_pParentEnvironment->CloneSelf(0));
}
@@ -368,7 +368,7 @@
_mapsessions[id] = state;
res.sessionid = id;
- RAVELOG_INFOA("started openrave session: %d, total: %d", id, _mapsessions.size());
+ RAVELOG_INFOA("started openrave session: %d, total: %d\n", id, _mapsessions.size());
return true;
}
Modified: pkg/trunk/openrave_planning/ormanipulation/data/ricebox.kinbody.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/ricebox.kinbody.xml 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/ormanipulation/data/ricebox.kinbody.xml 2008-12-20 03:33:29 UTC (rev 8479)
@@ -1,9 +1,14 @@
<Kinbody name="ricebox">
- <Body>
+ <Body name="base">
<Geom type="box">
- <extents>0.03 0.0525 0.1125</extents>
- <translation>0 0 0.1125</translation>
+ <extents>0.1125 0.0525 0.03</extents>
+ <translation>0 0 0.03</translation>
<diffusecolor>0.1 0.6 1</diffusecolor>
</Geom>
</Body>
+ <objecttransform>
+ <offsetlink>base</offsetlink>
+ <sid>data/ricebox.kinbody.xml</sid>
+ <pretranslation>-0.0155 0.0205 0</pretranslation>
+ </objecttransform>
</Kinbody>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/testscene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/testscene.m 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/testscene.m 2008-12-20 03:33:29 UTC (rev 8479)
@@ -3,8 +3,9 @@
cd(getenv('OCTAVE_WORKINGDIR'));
startup;
+openraveros_restart();
orEnvLoadScene('',1); % reset the scene
-#orEnvSetOptions('debug debug');
+orEnvSetOptions('debug debug');
%% create problem before everything so resources can init!
probs.rosplan = orEnvCreateProblem('ROSPlanningProblem');
@@ -14,6 +15,11 @@
orEnvLoadScene('data/pr2table_real.env.xml');
+out = orProblemSendCommand('createsystem ObjectTransform /checkerdetector/ObjectDetection 0.1',probs.rosplan);
+if( isempty(out) )
+ error('failed to create checkerboard detector');
+end
+
out = orProblemSendCommand('createsystem PhaseSpace phase_space_snapshot',probs.rosplan);
if( isempty(out) )
error('failed to create phasespace');
Modified: pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2008-12-20 03:33:29 UTC (rev 8479)
@@ -1,9 +1,13 @@
<launch>
+ <machine name="localhost" address="localhost" default="true"/>
+
<!-- start openraveros server and set all the correct paths -->
<include file="$(find ormanipulation)/startopenrave.ros.xml"/>
+ <include file="$(find ormanipulation)/startchecker.ros.xml"/>
+
<node pkg="phase_space" type="phase_space_node"/>
<!-- launch octave scripts to start the openrave client -->
- <node pkg="ormanipulation" type="testscene.m" output="screen">
+<!-- <node pkg="ormanipulation" type="testscene.m" output="screen">
<env name="OCTAVE_WORKINGDIR" value="$(find ormanipulation)/octave"/>
- </node>
+ </node> -->
</launch>
Added: pkg/trunk/openrave_planning/ormanipulation/startcameras.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startcameras.ros.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/startcameras.ros.xml 2008-12-20 03:33:29 UTC (rev 8479)
@@ -0,0 +1,19 @@
+<launch>
+ <machine name="cameras" address="ahc" default="false" user="rdiankov"/>
+ <node machine="cameras" name="dcam" pkg="dcam" type="dcam" respawn="false">
+ <!-- video_mode should be one of the following:
+ 640x480_videre_rectified: Provides rectified images from the hw
+ Provides: left mono image
+ 640x480_videre_disparity: Disparity and rectification done on chip.
+ Provides: Disparity and left mono image
+ 640x480_videre_disparity_raw: Disparity done on chip (debayer and rectification in software).
+ Provides: disparity and left color image.
+ 640x480_videre_none: No stereo on chip (all processing done in software).
+ Provides: all 3 images available
+ -->
+ <param name="video_mode" type="str" value="640x480_videre_rectified"/>
+ <param name="do_rectify" type="bool" value="False"/>
+ <param name="do_stereo" type="bool" value="False"/>
+ <param name="do_calc_points" type="bool" value="False"/>
+ </node>
+</launch>
Added: pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml 2008-12-20 03:33:29 UTC (rev 8479)
@@ -0,0 +1,33 @@
+<launch>
+ <machine name="cameras" address="ahc" default="false" user="rdiankov"/>
+
+ <node machine="cameras" name="dcam" pkg="dcam" type="dcam" respawn="false">
+ <!-- video_mode should be one of the following:
+ 640x480_videre_rectified: Provides rectified images from the hw
+ Provides: left mono image
+ 640x480_videre_disparity: Disparity and rectification done on chip.
+ Provides: Disparity and left mono image
+ 640x480_videre_disparity_raw: Disparity done on chip (debayer and rectification in software).
+ Provides: disparity and left color image.
+ 640x480_videre_none: No stereo on chip (all processing done in software).
+ Provides: all 3 images available
+ -->
+ <param name="video_mode" type="str" value="640x480_videre_rectified"/>
+ <param name="do_rectify" type="bool" value="False"/>
+ <param name="do_stereo" type="bool" value="False"/>
+ <param name="do_calc_points" type="bool" value="False"/>
+ </node>
+ <group ns="checkerdetector" clear_params="true">
+ <param name="display" type="int" value="1"/>
+ <param name="rect0_size_x" type="double" value="0.02133"/>
+ <param name="rect0_size_y" type="double" value="0.021"/>
+ <param name="grid0_size_x" type="int" value="4"/>
+ <param name="grid0_size_y" type="int" value="3"/>
+ <param name="type0" type="string" value="data/ricebox.kinbody.xml"/>
+ <node machine="cameras" pkg="checkerboard_detector" type="checkerboard_detector" respawn="true" output="screen">
+ <remap from="CamInfo" to="/dcam/left/cam_info"/>
+ <remap from="Image" to="/dcam/left/image_rect"/>
+ <env name="DISPLAY" value=":0.0"/>
+ </node>
+ </group>
+</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml 2008-12-20 03:33:29 UTC (rev 8479)
@@ -1,6 +1,8 @@
<launch>
+ <machine name="localhost-openrave" address="localhost"/>
+
<!-- start openraveros server and set all the correct paths -->
- <node pkg="openraveros" type="openraveros" output="screen">
+ <node machine="localhost-openrave" pkg="openraveros" type="openraveros" output="screen">
<env name="OPENRAVE_DATA" value="$(env OPENRAVE_DATA):$(find openrave)/share/openrave:$(find openrave_robot_description):$(find ormanipulation)"/>
<env name="OPENRAVE_PLUGINS" value="$(env OPENRAVE_PLUGINS):$(find openrave)/share/openrave/plugins:$(find orplugins)"/>
</node>
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2008-12-20 03:33:29 UTC (rev 8479)
@@ -10,4 +10,5 @@
<depend package="openrave_robot_description"/>
<depend package="libKinematics"/>
<depend package="boost"/>
+ <depend package="checkerboard_detector"/>
</package>
Added: pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2008-12-20 03:33:29 UTC (rev 8479)
@@ -0,0 +1,141 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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 OBJECTTRANSFORM_SENSOR_SYSTEM
+#define OBJECTTRANSFORM_SENSOR_SYSTEM
+
+#include "rossensorsystem.h"
+#include "checkerboard_detector/ObjectDetection.h"
+
+using namespace ros;
+
+// used to update objects through a mocap system
+class ObjectTransformXMLID
+{
+public:
+ static const char* GetXMLId() { return "objecttransform"; }
+};
+
+class ObjectTransformSystem : public ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>
+{
+public:
+ ObjectTransformSystem(EnvironmentBase* penv)
+ : ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>(penv)
+ {
+ }
+
+ virtual bool Init(istream& sinput)
+ {
+ _topic = "ObjectDetection";
+ bool bSuccess = ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>::Init(sinput);
+ if( !bSuccess )
+ return false;
+ _fThreshSqr = 0.05*0.05f;
+ sinput >> _fThreshSqr;
+ return true;
+ }
+
+private:
+ void newdatacb()
+ {
+ list< SNAPSHOT > listbodies;
+ list< const checkerboard_detector::Object6DPose* > listnewobjs;
+
+ {
+ boost::mutex::scoped_lock lock(_mutex);
+ TYPEOF(_mapbodies) mapbodies = _mapbodies;
+
+ FOREACHC(itobj, _topicmsg.objects) {
+ boost::shared_ptr<BODY> b;
+ Transform tnew = GetTransform(itobj->pose);
+
+ FOREACH(it, mapbodies) {
+ if( it->second->_initdata.sid == itobj->type ) {
+
+ // same type matched, need to check proximity
+ Transform tbody = it->second->GetOffsetLink()->GetParent()->GetTransform();
+ if( (tbody.trans-tnew.trans).lengthsqr3() > _fThreshSqr )
+ break;
+
+ b = it->second;
+ mapbodies.erase(it);
+ break;
+ }
+ }
+
+ if( !b ) {
+ listnewobjs.push_back(&(*itobj));
+ }
+ else {
+ if( !b->IsEnabled() )
+ continue;
+
+ listbodies.push_back(SNAPSHOT(b, tnew));
+ }
+ }
+
+ UpdateBodies(listbodies);
+ }
+
+ // try to add the left-over objects
+ if( listnewobjs.size() > 0 ) {
+ GetEnv()->LockPhysics(true);
+ FOREACH(itobj, listnewobjs) {
+
+ KinBody* pbody = GetEnv()->CreateKinBody();
+
+ if( !pbody->Init( (*itobj)->type.c_str(), NULL ) ) {
+ RAVELOG_ERRORA("failed to create object %s\n", (*itobj)->type.c_str());
+ delete pbody;
+ continue;
+ }
+
+ if( !GetEnv()->AddKinBody(pbody) ) {
+ RAVELOG_ERRORA("failed to add body %S\n", pbody->GetName());
+ delete pbody;
+ continue;
+ }
+
+ if( AddKinBody(pbody, NULL) == NULL ) {
+ delete pbody;
+ continue;
+ }
+
+ pbody->SetTransform(GetTransform((*itobj)->pose));
+ }
+ GetEnv()->LockPhysics(false);
+ }
+ }
+
+ 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));
+ }
+
+ dReal _fThreshSqr;
+};
+
+#endif
+
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2008-12-20 03:33:29 UTC (rev 8479)
@@ -27,6 +27,8 @@
#include "rosarmik.h"
#include "phasespacesystem.h"
+#include "objecttransformsystem.h"
+
#include "rosplanningproblem.h"
// declaring variables with stdcall can be a little complex
@@ -61,6 +63,8 @@
case PT_SensorSystem:
if( wcsicmp(name, L"PhaseSpace") == 0 )
return new PhaseSpaceMocapClient(penv);
+ if( wcsicmp(name, L"ObjectTransform") == 0 )
+ return new ObjectTransformSystem(penv);
default:
break;
}
@@ -78,6 +82,7 @@
pinfo->iksolvers.push_back(L"ROSArmIK");
pinfo->sensorsystems.push_back(L"PhaseSpace");
+ pinfo->sensorsystems.push_back(L"ObjectTransform");
pinfo->problems.push_back(L"ROSPlanningProblem");
return true;
Modified: pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h 2008-12-20 03:33:29 UTC (rev 8479)
@@ -26,141 +26,68 @@
#ifndef PHASESPACE_MOCAP_SYSTEM
#define PHASESPACE_MOCAP_SYSTEM
-#include "simplesensorsystem.h"
+#include "rossensorsystem.h"
#include "phase_space/PhaseSpaceSnapshot.h"
using namespace ros;
// used to update objects through a mocap system
-class PhaseSpaceMocapClient : public SimpleSensorSystem
+class PhaseSpaceXMLID
{
public:
- PhaseSpaceMocapClient(EnvironmentBase* penv) : SimpleSensorSystem(penv, "phasespace"), _bSubscribed(false)
+ static const char* GetXMLId() { return "phasespace"; }
+};
+
+class PhaseSpaceMocapClient : public ROSSensorSystem<phase_space::PhaseSpaceSnapshot, PhaseSpaceXMLID>
+{
+public:
+ PhaseSpaceMocapClient(EnvironmentBase* penv)
+ : ROSSensorSystem<phase_space::PhaseSpaceSnapshot, PhaseSpaceXMLID>(penv)
{
- RegisterXMLReader(GetEnv()); // just in case, register again
}
- virtual ~PhaseSpaceMocapClient() {
- Destroy();
- }
virtual bool Init(istream& sinput)
{
- if( !SimpleSensorSystem::Init(sinput) )
- return false;
-
- _phasespacetopic = "phase_space_snapshot";
- sinput >> _phasespacetopic;
- startsubscriptions();
- return _bSubscribed;
+ _topic = "phase_space_snapshot";
+ return ROSSensorSystem<phase_space::PhaseSpaceSnapshot, PhaseSpaceXMLID>::Init(sinput);
}
- virtual void Destroy()
- {
- stopsubscriptions();
- SimpleSensorSystem::Destroy();
- }
-
- static void RegisterXMLReader(EnvironmentBase* penv)
- {
- if( penv != NULL )
- penv->RegisterXMLReader("phasespace", PhaseSpaceMocapClient::CreateMocapReader);
- }
-
- static BaseXMLReader* CreateMocapReader(KinBody* parent, const char **atts)
- {
- return new MocapXMLReader("phasespace", NULL, atts);
- }
-
private:
- node* check_roscpp()
+ void newdatacb()
{
- // start roscpp
- ros::node* pnode = ros::node::instance();
+ list< SNAPSHOT > listbodies;
+ list< const phase_space::PhaseSpaceBody* > listnewbodies;
- if( pnode && !pnode->check_master() ) {
- ros::fini();
- delete pnode;
- return NULL;
- }
+ {
+ boost::mutex::scoped_lock lock(_mutex);
- if (!pnode) {
- int argc = 0;
- char strname[256] = "nohost";
- gethostname(strname, sizeof(strname));
- strcat(strname,"_rosoct");
+ for (unsigned int i=0; i<_topicmsg.get_bodies_size(); i++) {
+ const phase_space::PhaseSpaceBody& psbody = _topicmsg.bodies[i];
- ros::init(argc,NULL);
-
- pnode = new ros::node(strname, ros::node::DONT_HANDLE_SIGINT|ros::node::ANONYMOUS_NAME|ros::node::DONT_ADD_ROSOUT_APPENDER);
-
- bool bCheckMaster = pnode->check_master();
- ros::fini();
- delete pnode;
+ boost::shared_ptr<BODY> b;
+ Transform tnew = GetTransform(psbody.pose);
- if( !bCheckMaster ) {
- RAVELOG_ERRORA("ros not present");
- return NULL;
- }
-
- ros::init(argc,NULL);
- pnode = new ros::node(strname, ros::node::DONT_HANDLE_SIGINT|ros::node::ANONYMOUS_NAME);
- RAVELOG_DEBUGA("new roscpp node started");
- }
+ FOREACH(it, _mapbodies) {
+ if( it->second->_initdata.id == psbody.id ) {
+ b = it->second;
+ break;
+ }
+ }
- return pnode;
- }
-
- void startsubscriptions()
- {
- // check if thread launched
- _bSubscribed = false;
- ros::node* pnode = check_roscpp();
- if( pnode != NULL ) {
- _bSubscribed = pnode->subscribe(_phasespacetopic, _snapshot, &PhaseSpaceMocapClient::newdatacb, this, 10);
- if( _bSubscribed )
- RAVELOG_DEBUGA("subscribed to %s\n", _phasespacetopic.c_str());
- else
- RAVELOG_ERRORA("failed to subscribe to %s\n", _phasespacetopic.c_str());
- }
- }
-
- void stopsubscriptions()
- {
- if( _bSubscribed ) {
- ros::node* pnode = ros::node::instance();
- if( pnode && pnode->check_master() ) {
- pnode->unsubscribe(_phasespacetopic.c_str());
- RAVELOG_DEBUGA("unsubscribe from %s\n", _phasespacetopic.c_str());
- }
- _bSubscribed = false;
- }
- }
-
- void newdatacb()
- {
- list< SNAPSHOT > listbodies;
- boost::mutex::scoped_lock lock(_mutex);
- RAVELOG_VERBOSEA("cb\n");
-
- for (unsigned int i=0; i<_snapshot.get_bodies_size(); i++) {
- const phase_space::PhaseSpaceBody& psbody = _snapshot.bodies[i];
-
- boost::shared_ptr<BODY> b;
- FOREACH(it, _mapbodies) {
- if( it->second->_initdata.id == psbody.id ) {
- b = it->second;
- break;
+ if( !b ) {
+ listnewbodies.push_back(&psbody);
}
+ else {
+ if( !b->IsEnabled() )
+ continue;
+
+ listbodies.push_back(SNAPSHOT(b, tnew));
+ }
}
- if( !b || !b->IsEnabled() )
- continue;
-
- listbodies.push_back(SNAPSHOT(b, GetTransform(psbody.pose)));
+ UpdateBodies(listbodies);
}
- UpdateBodies(listbodies);
-
// try to add the left-over objects
}
@@ -168,10 +95,6 @@
{
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));
}
-
- phase_space::PhaseSpaceSnapshot _snapshot;
- string _phasespacetopic;
- bool _bSubscribed;
};
#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2008-12-20 03:33:29 UTC (rev 8479)
@@ -34,6 +34,7 @@
#define FOREACH(it, v) for(BOOST_TYPEOF(v)::iterator it = (v).begin(); it != (v).end(); (it)++)
#define FOREACHC(it, v) for(BOOST_TYPEOF(v)::const_iterator it = (v).begin(); it != (v).end(); (it)++)
#define RAVE_REGISTER_BOOST
+#define TYPEOF BOOST_TYPEOF
#else
#include <string>
@@ -44,7 +45,7 @@
#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
#define FOREACHC FOREACH
-
+#define TYPEOF typeof
#endif
#include <fstream>
@@ -159,4 +160,48 @@
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
+inline ros::node* check_roscpp()
+{
+ // start roscpp
+ ros::node* pnode = ros::node::instance();
+
+ if( pnode && !pnode->check_master() ) {
+ ros::fini();
+ delete pnode;
+ return NULL;
+ }
+
+ if (!pnode) {
+ int argc = 0;
+ char strname[256] = "nohost";
+ gethostname(strname, sizeof(strname));
+ strcat(strname,"_openrave");
+
+ ros::init(argc,NULL);
+
+ pnode = new ros::node(strname, ros::node::DONT_HANDLE_SIGINT|ros::node::ANONYMOUS_NAME|ros::node::DONT_ADD_ROSOUT_APPENDER);
+
+ bool bCheckMaster = pnode->check_master();
+ ros::fini();
+ delete pnode;
+
+ if( !bCheckMaster ) {
+ RAVELOG_ERRORA("ros not present");
+ return NULL;
+ }
+
+ ros::init(argc,NULL);
+ pnode = new ros::node(strname, ros::node::DONT_HANDLE_SIGINT|ros::node::ANONYMOUS_NAME);
+ RAVELOG_DEBUGA("new roscpp node started");
+ }
+
+ return pnode;
+}
+
+inline ros::node* check_roscpp_nocreate()
+{
+ ros::node* pnode = ros::node::instance();
+ return (pnode && pnode->check_master()) ? pnode : NULL;
+}
+
#endif
Added: pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2008-12-20 03:33:29 UTC (rev 8479)
@@ -0,0 +1,98 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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 ROS_SENSORSYSTEM_SYSTEM
+#define ROS_SENSORSYSTEM_SYSTEM
+
+#include "simplesensorsystem.h"
+
+using namespace ros;
+
+// used to update objects through a mocap system
+template <typename T, typename XMLID>
+class ROSSensorSystem : public SimpleSensorSystem<XMLID>
+{
+public:
+ ROSSensorSystem(EnvironmentBase* penv) : SimpleSensorSystem<XMLID>(penv), _bSubscribed(false)
+ {
+
+ }
+ virtual ~ROSSensorSystem() {
+ Destroy();
+ }
+
+ virtual bool Init(istream& sinput)
+ {
+ if( !SimpleSensorSystem<XMLID>::Init(sinput) )
+ return false;
+
+ sinput >> _topic;
+ startsubscriptions();
+ return _bSubscribed;
+ }
+
+ virtual void Destroy()
+ {
+ stopsubscriptions();
+ SimpleSensorSystem<XMLID>::Destroy();
+ }
+
+protected:
+ virtual void startsubscriptions()
+ {
+ // check if thread launched
+ _bSubscribed = false;
+ ros::node* pnode = check_roscpp();
+ if( pnode != NULL ) {
+ _bSubscribed = pnode->subscribe(_topic, _topicmsg, &ROSSensorSystem::newdatacb, this, 10);
+ if( _bSubscribed )
+ RAVELOG_DEBUGA("subscribed to %s\n", _topic.c_str());
+ else
+ RAVELOG_ERRORA("failed to subscribe to %s\n", _topic.c_str());
+ }
+ }
+
+ virtual void stopsubscriptions()
+ {
+ if( _bSubscribed ) {
+ ros::node* pnode = check_roscpp_nocreate();
+ if( pnode != NULL ) {
+ pnode->unsubscribe(_topic.c_str());
+ RAVELOG_DEBUGA("unsubscribe from %s\n", _topic.c_str());
+ }
+ _bSubscribed = false;
+ }
+ }
+
+ virtual void newdatacb()
+ {
+ }
+
+ T _topicmsg;
+ string _topic;
+ bool _bSubscribed;
+};
+
+#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2008-12-20 03:25:40 UTC (rev 8478)
+++ pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2008-12-20 03:33:29 UTC (rev 8479)
@@ -29,28 +29,27 @@
using namespace std;
// used to update objects through a mocap system
+template <typename XMLID>
class SimpleSensorSystem : public SensorSystemBase
{
public:
class MocapData : public XMLReadable
{
public:
- MocapData(const string& xmlid) : _xmlid(xmlid) {}
- virtual const char* GetXMLId() { return _xmlid.c_str(); }
+ MocapData() {}
+ virtual const char* GetXMLId() { return XMLID::GetXMLId(); }
string sid; ///< global id for the system id
int id;
+ string tfframe; ///< if not empty, the tf frame this object is in
std::wstring strOffsetLink; ///< the link where the markers are attached (if any)
Transform transOffset,transPreOffset; // final offset = transOffset * transReturnedFromVision * transPreOffset
-
- private:
- string _xmlid;
};
class BODY : public BODYBASE
{
public:
- BODY(KinBody* pbody, const MocapData& initdata, const string& xmlid) : BODYBASE(), _initdata(xmlid)
+ BODY(KinBody* pbody, const MocapData& initdata) : BODYBASE()
{
assert( pbody != NULL );
_initdata = initdata;
@@ -69,15 +68,18 @@
virtual void* GetInitData(int* psize) { if( psize ) *psize = sizeof(_initdata); return &_initdata; }
ros::Time lastupdated;
MocapData _initdata;
+
+ private:
+ friend class SimpleSensorSystem<XMLID>;
};
class MocapXMLReader : public BaseXMLReader
{
public:
- MocapXMLReader(const string& xmlid, MocapData* pMocap, const char **atts) : _xmlid(xmlid) {
+ MocapXMLReader(MocapData* pMocap, const char **atts) {
_pMocap = pMocap;
if( _pMocap == NULL )
- _pMocap = new MocapData(_xmlid);
+ _pMocap = new MocapData();
}
virtual ~MocapXMLReader() { delete _pMocap; }
@@ -86,7 +88,7 @@
virtual void startElement(void *ctx, const char *name, const char **atts) {}
virtual bool endElement(void *ctx, const char *name)
{
- if( stricmp((const char*)name, _xmlid.c_str()) == 0 )
+ if( stricmp((const char*)name, XMLID::GetXMLId()) == 0 )
return true;
if( stricmp((const char*)name, "offsetlink") == 0 ) {
@@ -98,6 +100,8 @@
ss >> _pMocap->id;
else if( stricmp((const char*)name, "sid") == 0 )
ss >> _pMocap->sid;
+ else if( stricmp((const char*)name, "tfframe") == 0 )
+ ss >> _pMocap->tfframe;
else if( stricmp((const char*)name, "translation") == 0 )
ss >> _pMocap->transOffset.trans.x >> _pMocap->transOffset.trans.y >> _pMocap->transOffset.trans.z;
else if( stricmp((const char*)name, "rotationmat") == 0 ) {
@@ -148,12 +152,24 @@
protected:
MocapData* _pMocap;
stringstream ss;
- string _xmlid;
};
- SimpleSensorSystem(EnvironmentBase* penv, const string& xmlid) : SensorSystemBase(penv), _expirationtime(2,0), _xmlid(xmlid)
+ static void RegisterXMLReader(EnvironmentBase* penv)
{
+ if( penv != NULL )
+ penv->RegisterXMLReader(XMLID::GetXMLId(), SimpleSensorSystem<XMLID>::CreateMocapReader);
}
+
+ static BaseXMLReader* CreateMocapReader(KinBody* parent, const char **atts)
+ {
+ return new MocapXMLReader(NULL, atts);
+ }
+
+ SimpleSensorSystem(EnvironmentBase* penv) : SensorSystemBase(penv), _expirationtime(2,0)
+ {
+ RegisterXMLReader(GetEnv()); // just in case, register again
+ }
+
virtual ~SimpleSensorSystem() {
Destroy();
}
@@ -173,7 +189,7 @@
{
// go through all bodies in the environment and check for mocap data
FOREACHC(itbody, vbodies) {
- MocapData* pmocapdata = (MocapData*)((*itbody)->GetExtraInterface(GetXMLId()));
+ MocapData* pmocapdata = (MocapData*)((*itbody)->GetExtraInterface(XMLID::GetXMLId()));
if( pmocapdata != NULL ) {
BODY* p = AddKinBody(*itbody, pmocapdata);
if( p != NULL ) {
@@ -186,11 +202,18 @@
virtual BODY* AddKinBody(KinBody* pbody, const void* _pdata)
{
- if( _pdata == NULL || pbody == NULL )
+ if( pbody == NULL )
return false;
assert( pbody->GetEnv() == GetEnv() );
const MocapData* pdata = (const MocapData*)_pdata;
+ if( pdata == NULL ) {
+ pdata = (const MocapData*)(pbody->GetExtraInterface(XMLID::GetXMLId()));
+ if( pdata == NULL ) {
+ RAVELOG_ERRORA("failed to find mocap data for body %S\n", pbody->GetName());
+ return NULL;
+ }
+ }
boost::mutex::scoped_lock lock(_mutex);
if( _mapbodies.find(pbody->GetNetworkId()) != _mapbodies.end() ) {
@@ -198,9 +221,9 @@
return NULL;
}
- BODY* b = new BODY(pbody, *pdata, _xmlid);
+ BODY* b = new BODY(pbody, *pdata);
_mapbodies[pbody->GetNetworkId()].reset(b);
- RAVELOG_DEBUGA("system adding body %S\n", pbody->GetName());
+ RAVELOG_DEBUGA("system adding body %S, total: %d\n", pbody->GetName(), _mapbodies.size());
return b;
}
@@ -233,7 +256,7 @@
return false;
assert( pbody->GetEnv() == GetEnv() );
- map<int,boost::shared_ptr<BODY> >::iterator it = _mapbodies.find(pbody->GetNetworkId());
+ TYPEOF(_mapbodies.begin()) it = _mapbodies.find(pbody->GetNetworkId());
if( it == _mapbodies.end() ) {
RAVELOG_WARNA("trying to %s body %S that is not in system\n", bEnable?"enable":"disable", pbody->GetName());
return false;
@@ -250,12 +273,10 @@
assert( pbody->GetEnv() == GetEnv() );
boost::mutex::scoped_lock lock(_mutex);
- map<int,boost::shared_ptr<BODY> >::iterator it = _mapbodies.find(pbody->GetNetworkId());
+ TYPEOF(_mapbodies.begin()) it = _mapbodies.find(pbody->GetNetworkId());
return it != _mapbodies.end() ? it->second.get() : NULL;
}
- virtual const char* GetXMLId() { return _xmlid.c_str(); }
-
protected:
typedef pair<boost::shared_ptr<BODY>, Transform > SNAPSHOT;
@@ -276,11 +297,11 @@
TransformMatrix tbase = it->first->GetOffsetLink()->GetParent()->GetTransform();
TransformMatrix toffset = tbase * tlink.inverse() * it->first->_initdata.transOffset;
TransformMatrix tfinal = toffset * it->second*it->first->_initdata.transPreOffset;
-
+
it->first->GetOffsetLink()->GetParent()->SetTransform(tfinal);
it->first->lastupdated = curtime;
- RAVELOG_DEBUGA("%f %f %f\n", tfinal.trans.x, tfinal.trans.y, tfinal.trans.z);
+ //RAVELOG_DEBUGA("%f %f %f\n", tfinal.trans.x, tfinal.trans.y, tfinal.trans.z);
if( !it->first->IsPresent() )
RAVELOG_VERBOSEA("updating body %S\n", it->first->GetOffsetLink()->GetParent()->GetName());
@@ -289,10 +310,11 @@
GetEnv()->LockPhysics(false);
- map<int,boost::shared_ptr<BODY> >::iterator itbody = _mapbodies.begin();
+ TYPEOF(_mapbodies.begin()) itbody = _mapbodies.begin();
while(itbody != _mapbodies.end()) {
if( curtime-itbody->second->lastupdated > _expirationtime ) {
if( !itbody->second->IsLocked() ) {
+ RAVELOG_DEBUGA("object %S expired %fs\n", itbody->second->GetOffsetLink()->GetParent(), (float)(curtime-itbody->second->lastupdated).toSec());
GetEnv()->RemoveKinBody(itbody->second->GetOffsetLink()->GetParent());
_mapbodies.erase(itbody++);
continue;
@@ -310,14 +332,13 @@
map<int,boost::shared_ptr<BODY> > _mapbodies;
boost::mutex _mutex;
ros::Duration _expirationtime;
- string _xmlid;
};
#ifdef RAVE_REGISTER_BOOST
#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()
-BOOST_TYPEOF_REGISTER_TYPE(SimpleSensorSystem::MocapData)
-BOOST_TYPEOF_REGISTER_TYPE(SimpleSensorSystem::BODY)
-BOOST_TYPEOF_REGISTER_TYPE(SimpleSensorSystem::MocapXMLReader)
+BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::MocapData, 1)
+BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::BODY, 1)
+BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::MocapXMLReader, 1)
#endif
#endif
Added: pkg/trunk/vision/checkerboard_detector/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/checkerboard_detector/CMakeLists.txt (rev 0)
+++ pkg/trunk/vision/checkerboard_detector/CMakeLists.txt 2008-12-20 03:33:29 UTC (rev 8479)
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(checkerboard_detector)
+rospack_add_executable(checkerboard_detector checkerboard_detector.cpp)
+
+include(CheckIncludeFile)
+include(CheckCXXCompilerFlag)
+include(CheckLibraryExists)
+
+if( WIN32 )
+ CHECK_INCLUDE_FILE(omp.h HAVE_OMP_H)
+ if( HAVE_OMP_H )
+ message(STATUS "Using OpenMP")
+ check_cxx_compiler_flag(/openmp HAVE_OPENMP)
+
+ if( HAVE_OPENMP )
+ add_definitions("/openmp")
+ endif()
+ endif()
+elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
+
+ # check if compilers supports -fopenmp
+ check_cxx_compiler_flag(-fopenmp HAVE_OPENMP)
+ check_library_exists(gomp omp_get_num_threads "" HAS_GOMP_LIB)
+
+ if( HAVE_OPENMP AND HAS_GOMP_LIB )
+ add_definitions("-fopenmp")
+ target_link_libraries(checkerboard_detector gomp)
+ set(OPENMP_LFLAGS "-lgomp")
+ endif()
+endif()
+
+genmsg()
+
+# if( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX )
+# add_definitions(" -msse2 -O2 ")
+# endif()
Added: pkg/trunk/vision/checkerboard_detector/Makefile
===================================================================
--- pkg/trunk/vision/checkerboard_detector/Makefile (rev 0)
+++ pkg/trunk/vision/checkerboard_detector/Makefile 2008-12-20 03:33:29 UTC (rev 8479)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
===================================================================
--- pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp (rev 0)
+++ pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp 2008-12-20 03:33:29 UTC (rev 8479)
@@ -0,0 +1,357 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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
+#include <cstdio>
+#include <vector>
+#include <ros/node.h>
+
+#include <boost/thread/mutex.hpp>
+
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+#include "image_msgs/CvBridge.h"
+#include "image_msgs/StereoInfo.h"
+#include "image_msgs/CamInfo.h"
+#include "image_msgs/Image.h"
+#include "checkerboard_detector/ObjectDetection.h"
+#include "math.h"
+
+#include <sys/timeb.h> // ftime(), struct timeb
+#include <sys/time.h>
+
+using namespace std;
+using namespace ros;
+
+class CheckerboardDetectorNode : public node
+{
+public:
+ struct CHECKERBOARD
+ {
+ CvSize griddims; ///< number of squares
+ vector<Vector> grid3d;
+ vector<CvPoint2D32f> corners;
+ };
+
+ image_msgs::CamInfo _caminfomsg;
+ image_msgs::Image _imagemsg;
+ checkerboard_detector::ObjectDetection _objdetmsg;
+ image_msgs::CvBridge _cvbridge;
+
+ int display, uidnext;
+ vector<CHECKERBOARD> vcheckers; // grid points for every checkerboard
+ vector< string > vstrtypes; // type names for every grid point
+ map<string,int> maptypes;
+ ros::Time lasttime;
+ CvMat *intrinsic_matrix; // intrinsic matrices
+ boost::mutex _mutexcalib;
+ IplImage* frame;
+
+ CheckerboardDetectorNode() : node("checkerboard_detector"), uidnext(1), intrinsic_matrix(NULL), frame(NULL)
+ {
+ param("display",display,1);
+
+ char str[32];
+ int index = 0;
+ while(1) {
+ int dimx, dimy;
+ double fRectSize[2];
+ string type;
+
+ sprintf(str,"grid%d_size_x",index);
+ if( !get_param(str,dimx) )
+ break;
+
+ sprintf(str,"grid%d_size_y",index);
+ if( !get_param(str,dimy) )
+ break;
+
+ sprintf(str,"rect%d_size_x",index);
+ if( !get_param(str,fRectSize[0]) )
+ break;
+
+ sprintf(str,"rect%d_size_y",index);
+ if( !get_param(str,fRectSize[1]) )
+ break;
+
+ sprintf(str,"type%d",index);
+ if( !get_param(str,type) ) {
+ sprintf(str,"checker%dx%d", dimx, dimy);
+ type = str;
+ }
+
+ CHECKERBOARD cb;
+ cb.griddims = cvSize(dimx,dimy);
+
+ cb.grid3d.resize(dimx*dimy);
+ int j=0;
+ for(int y=0; y<dimy; ++y)
+ for(int x=0; x<dimx; ++x)
+ cb.grid3d[j++] = Vector(x*fRectSize[0], y*fRectSize[1], 0);
+
+ vcheckers.push_back(cb);
+
+ vstrtypes.push_back(type);
+ maptypes[vstrtypes.back()] = index;
+ index++;
+ }
+
+ if( maptypes.size() == 0 ) {
+ ROS_ERROR("no checkerboards to detect");
+ return;
+ }
+
+ if( display ) {
+ cvNamedWindow("Checkerboard Detector", CV_WINDOW_AUTOSIZE);
+ cvStartWindowThread();
+ }
+
+ lasttime = ros::Time::now();
+ advertise<checkerboard_detector::ObjectDetection>("ObjectDetection",1);
+ subscribe("CamInfo", _caminfomsg, &CheckerboardDetectorNode::caminfo_cb,1);
+ subscribe("Image", _imagemsg, &CheckerboardDetectorNode::image_cb,1);
+ }
+ ~CheckerboardDetectorNode()
+ {
+ if( frame )
+ cvReleaseImage(&frame);
+ if( intrinsic_matrix )
+ cvReleaseMat(&intrinsic_matrix);
+ }
+
+ void caminfo_cb()
+ {
+ boost::mutex::scoped_lock lock(_mutexcalib);
+ if( intrinsic_matrix == NULL )
+ intrinsic_matrix = cvCreateMat(3,3,CV_32FC1);
+
+ for(int i = 0; i < 3; ++i)
+ for(int j = 0; j < 3; ++j)
+ intrinsic_matrix->data.fl[3*i+j] = _caminfomsg.P[4*i+j];
+ }
+
+ void image_cb()
+ {
+ boost::mutex::scoped_lock lock(_mutexcalib);
+ if( intrinsic_matrix == NULL ) {
+ ROS_ERROR("need CamInfo message for calibration info");
+ return;
+ }
+
+ if( !_cvbridge.fromImage(_imagemsg, "mono") ) {
+ ROS_ERROR("failed to get image");
+ return;
+ }
+
+ IplImage *pimggray = _cvbridge.toIpl();
+ if( display ) {
+ // copy the raw image
+ if( frame != NULL && (frame->width != _caminfomsg.width || frame->height != _caminfomsg.height) ) {
+ cvReleaseImage(&frame);
+ frame = NULL;
+ }
+ if( frame == NULL )
+ frame = cvCreateImage(cvSize(_caminfomsg.width,_caminfomsg.height),IPL_DEPTH_8U, 3);
+
+ cvCvtColor(pimggray,frame,CV_GRAY2RGB);
+ }
+
+ vector<checkerboard_detector::Object6DPose> vobjects;
+
+ #pragma omp parallel for schedule(dynamic,1)
+ for(size_t i = 0; i < vcheckers.size(); ++i) {
+ CHECKERBOARD& cb = vcheckers[i];
+ int ncorners;
+ checkerboard_detector::Object6DPose objpose;
+
+ // do until no more checkerboards detected
+ while(1) {
+ cb.corners.resize(200);
+ int allfound = cvFindChessboardCorners( pimggray, cb.griddims, &cb.corners[0], &ncorners,
+ CV_CALIB_CB_ADAPTIVE_THRESH );
+ cb.corners.resize(ncorners);
+
+ //cvDrawChessboardCorners(pimgGray, itbox->second.griddims, &corners[0], ncorners, allfound);
+ //cvSaveImage("temp.jpg", pimgGray);
+
+ if(!allfound || ncorners != (int)cb.grid3d.size())
+ break;
+
+ // remove any corners that are close to the border
+ const int borderthresh = 30;
+ for(int j = 0; j < ncorners; ++j) {
+ int x = cb.corners[j].x;
+ int y = cb.corners[j].y;
+ if( x < borderthresh || x > pimggray->width-borderthresh ||
+ y < borderthresh || y > pimggray->height-borderthresh ) {
+ allfound = 0;
+ break;
+ }
+ }
+
+ // mark out the image
+ CvPoint upperleft, lowerright;
+ upperleft.x = lowerright.x = cb.corners[0].x;
+ upperleft.y = lowerright.y = cb.corners[0].y;
+ for(int j = 1; j < (int)cb.corners.size(); ++j) {
+ if( upperleft.x > cb.corners[j].x ) upperleft.x = cb.corners[j].x;
+ if( upperleft.y > cb.corners[j].y ) upperleft.y = cb.corners[j].y;
+ if( lowerright.x < cb.corners[j].x ) lowerright.x = cb.corners[j].x;
+ if( lowerright.y < cb.corners[j].y ) lowerright.y = cb.corners[j].y;
+ }
+
+ if( allfound ) {
+
+ cvFindCornerSubPix(pimggray, &cb.corners[0], cb.corners.size(), cvSize(5,5),cvSize(-1,-1),
+ cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2));
+
+ objpose.pose = FindTransformation(cb.corners, cb.grid3d);
+ }
+
+ #pragma omp critical
+ {
+ if( allfound ) {
+ objpose.uid = uidnext++;
+ vobjects.push_back(objpose);
+ vobjects.back().type = vstrtypes[i];
+ }
+
+ cvRectangle(pimggray, upperleft, lowerright, CV_RGB(0,0,0),CV_FILLED);
+ }
+ }
+
+ //cvSaveImage("temp.jpg", pimggray);
+ }
+
+ _objdetmsg.set_objects_vec(vobjects);
+ publish("ObjectDetection", _objdetmsg);
+
+ ROS_INFO("checkerboard: image: %dx%d (size=%d), num: %d, total: %.3fs",_caminfomsg.width,_caminfomsg.height,
+ _imagemsg.byte_data.data.size(), _objdetmsg.get_objects_size(),
+ (float)(ros::Time::now()-lasttime).toSec());
+ lasttime = ros::Time::now();
+
+ if( display ) {
+
+ // draw each found checkerboard
+ for(size_t i = 0; i < vobjects.size(); ++i) {
+ int itype = maptypes[vobjects[i].type];
+ CHECKERBOARD& cb = vcheckers[itype];
+ CvSize& s = cb.griddims;
+ Transform tlocal;
+ tlocal.trans = Vector(vobjects[i].pose.translation.x,vobjects[i].pose.translation.y,vobjects[i].pose.translation.z);
+ tlocal.rot = Vector(vobjects[i].pose.rotation.w,vobjects[i].pose.rotation.x,vobjects[i].pose.rotation.y, vobjects[i].pose.rotation.z);
+
+ CvPoint X[4];
+
+ int inds[4] = {0, s.width-1, s.width*(s.height-1), s.width*s.height-1 };
+
+ for(int i = 0; i < 4; ++i) {
+ Vector p = tlocal * cb.grid3d[inds[i]];
+ dReal fx = p.x*_caminfomsg.P[0] + p.y*_caminfomsg.P[1] + p.z*_caminfomsg.P[2] + _caminfomsg.P[3];
+ dReal fy = p.x*_caminfomsg.P[4] + p.y*_caminfomsg.P[5] + p.z*_caminfomsg.P[6] + _caminfomsg.P[7];
+ dReal fz = p.x*_caminfomsg.P[8] + p.y*_caminfomsg.P[9] + p.z*_caminfomsg.P[10] + _caminfomsg.P[11];
+ X[i].x = (int)(fx/fz);
+ X[i].y = (int)(fy/fz);
+ }
+
+ // draw two lines
+ CvScalar col0 = CV_RGB(255,0,(64*itype)%256);
+ CvScalar col1 = CV_RGB(0,255,(64*itype)%256);
+ cvLine(frame, X[0], X[1], col0, 1);
+ cvLine(frame, X[0], X[2], col1, 1);
+
+ // draw all the points
+ for(size_t i = 0; i < cb.grid3d.size(); ++i) {
+ Vector p = tlocal * cb.grid3d[i];
+ dReal fx = p.x*_caminfomsg.P[0] + p.y*_caminfomsg.P[1] + p.z*_caminfomsg.P[2] + _caminfomsg.P[3];
+ dReal fy = p.x*_caminfomsg.P[4] + p.y*_caminfomsg.P[5] + p.z*_caminfomsg.P[6] + _caminfomsg.P[7];
+ dReal fz = p.x*_caminfomsg.P[8] + p.y*_caminfomsg.P[9] + p.z*_caminfomsg.P[10] + _caminfomsg.P[11];
+ int x = (int)(fx/fz);
+ int y = (int)(fy/fz);
+ cvCircle(frame, cvPoint(x,y), 6, CV_RGB(0,0,0), 2);
+ cvCircle(frame, cvPoint(x,y), 2, CV_RGB(0,0,0), 2);
+ cvCircle(frame, cvPoint(x,y), 4, CV_RGB(128,128,64*itype), 3);
+ }
+
+ cvCircle(frame, X[0], 3, CV_RGB(255,255,128), 3);
+ }
+
+ cvShowImage("Checkerboard Detector",frame);
+ }
+ }
+
+ std_msgs::Transform FindTransformation(const vector<CvPoint2D32f> &imgpts, const vector<Vector> &objpts)
+ {
+ CvMat *objpoints = cvCreateMat(3,objpts.size(),CV_32FC1);
+ for(size_t i=0; i<objpts.size(); ++i) {
+ cvSetReal2D(objpoints, 0,i, objpts[i].x);
+ cvSetReal2D(objpoints, 1,i, objpts[i].y);
+ cvSetReal2D(objpoints, 2,i, objpts[i].z);
+ }
+
+ std_msgs::Transform pose;
+ float fR3[3];
+ CvMat R3, T3;
+ cvInitMatHeader(&R3, 3, 1, CV_32FC1, fR3);
+ cvInitMatHeader(&T3, 3, 1, CV_64FC1, &pose.translation.x);
+
+ // for some reason distortion coeffs are needed
+ float kc[4] = {0};
+ CvMat kcmat;
+ cvInitMatHeader(&kcmat,1,4,CV_32FC1,kc);
+
+ CvMat img_points;
+ cvInitMatHeader(&img_points, 1,imgpts.size(), CV_32FC2, const_cast<CvPoint2D32f*>(imgpts.data()));
+
+ cvFindExtrinsicCameraParams2(objpoints, &img_points, intrinsic_matrix, &kcmat, &R3, &T3);
+ cvReleaseMat(&objpoints);
+
+ double fang = sqrt(fR3[0]*fR3[0] + fR3[1]*fR3[1] + fR3[2]*fR3[2]);
+ if( fang < 1e-6 ) {
+ pose.rotation.w = 1;
+ pose.rotation.x = 0;
+ pose.rotation.y = 0;
+ pose.rotation.z = 0;
+ }
+ else {
+ double fmult = sin(fang/2)/fang;
+ pose.rotation.w = cos(fang/2);
+ pose.rotation.x = fR3[0]*fmult;
+ pose.rotation.y = fR3[1]*fmult;
+ pose.rotation.z = fR3[2]*fmult;
+ }
+
+ return pose;
+ }
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ CheckerboardDetectorNode checker;
+ checker.spin();
+ ros::fini();
+ return 0;
+}
Added: pkg/trunk/vision/checkerboard_detector/manifest.xml
===================================================================
--- pkg/trunk/vision/checkerboard_detector/manifest.xml (rev 0)
+++ pkg/trunk/vision/checkerboard_detector/manifest.xml 2008-12-20 03:33:29 UTC (rev 8479)
@@ -0,0 +1,23 @@
+<package>
+ <description brief="Finds checkerboards and returns their 6D poses">
+ Uses opencv to find checkboards and compute their 6D poses with respect to the image. Requires the image to be calibrated.
+ Parameters:
+ display - show the checkerboard detection
+ rect%d_size_x - size of checker in x direction
+ rect%d_size_y - size of checker in y direction
+ grid%d_size_x - number of checkers in x direction
+ grid%d_size_y - number of checkers in y direction
+
+ There can be more than one grid%d declared, the numbers should grow consecutively starting at 0.
+ </description>
+ <author>Rosen Diankov (rdi...@cs...)</author>
+ <license>BSD</license>
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp"/>
+ </export>
+ <depend package="roscpp"/>
+ <depend package="boost"/>
+ <depend package="std_msgs"/>
+ <depend package="opencv_latest"/>
+ <depend package="image_msgs"/>
+</package>
Added: pkg/trunk/vision/checkerboard_detector/math.h
===================================================================
--- pkg/trunk/vision/checkerboard_detector/math.h (rev 0)
+++ pkg/trunk/vision/checkerboard_detector/math.h 2008-12-20 03:33:29 UTC (rev 8479)
@@ -0,0 +1,1801 @@
+// Copyright (C) 2003-2008 Rosen Diankov
+// Computer Science Department
+// Carnegie Mellon University, Robotics Institute
+// 5000 Forbes Ave. Pittsburgh, PA 15213
+// U.S.A.
+//
+// This file is part of OpenRAVE.
+// OpenRAVE is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+/*! --------------------------------------------------------------------
+ \file math.h
+ \brief Defines basic math and gemoetric primitives. dReal is defined in ODE. Any
+ functions not in here like conversions between quaterions
+ and matrices are most likely defined in ODE.
+ Originally from OpenRAVE (http://openrave.programmingvision.com)
+ -------------------------------------------------------------------- */
+
+#...
[truncated message content] |
