Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2557] pkg/trunk
|
From: <is...@us...> - 2008-08-05 18:18:22
|
Revision: 2557
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2557&view=rev
Author: isucan
Date: 2008-08-05 18:18:28 +0000 (Tue, 05 Aug 2008)
Log Message:
-----------
world_3d_map now has support for removing points from pointclouds if they collide with the robot's arms
Modified Paths:
--------------
pkg/trunk/util/collision_space/include/collision_space/util.h
pkg/trunk/world_models/world_3d_map/manifest.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/util/collision_space/include/collision_space/util.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/util.h 2008-08-05 17:39:31 UTC (rev 2556)
+++ pkg/trunk/util/collision_space/include/collision_space/util.h 2008-08-05 18:18:28 UTC (rev 2557)
@@ -57,26 +57,48 @@
m_scale = scale;
}
- bool containsPoint(const libTF::Pose3D &pose, const libTF::Pose3D::Position &p) const
+ void setPose(const libTF::Pose3D &pose)
{
+ m_pose = pose;
+ m_pose.invert();
+ }
+
+ bool containsPoint(const libTF::Pose3D::Position &p) const
+ {
/* bring point in the reference frame described by pose */
libTF::Pose3D::Position pt = p;
- pose.applyToPosition(pt);
+ m_pose.applyToPosition(pt);
/* since the body is centered at origin, scaling the body is equivalent
* to scaling the coordinates of the point */
- pt.x *= m_scale;
- pt.y *= m_scale;
- pt.z *= m_scale;
+ pt.x /= m_scale;
+ pt.y /= m_scale;
+ pt.z /= m_scale;
- return containsPoint(pt);
+ return containsPt(pt);
}
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const = 0;
-
+ bool containsPoint(double x, double y, double z) const
+ {
+ /* bring point in the reference frame described by pose */
+ libTF::Pose3D::Position pt = { x, y, z };
+ m_pose.applyToPosition(pt);
+
+ /* since the body is centered at origin, scaling the body is equivalent
+ * to scaling the coordinates of the point */
+ pt.x /= m_scale;
+ pt.y /= m_scale;
+ pt.z /= m_scale;
+
+ return containsPt(pt);
+ }
+
protected:
+
+ virtual bool containsPt(const libTF::Pose3D::Position &p) const = 0;
- double m_scale;
+ libTF::Pose3D m_pose;
+ double m_scale;
};
@@ -97,13 +119,13 @@
m_radius = radius;
m_radius2 = radius * radius;
}
-
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+
+ protected:
+
+ virtual bool containsPt(const libTF::Pose3D::Position &p) const
{
return p.x * p.x + p.y * p.y + p.z * p.z < m_radius2;
}
-
- protected:
double m_radius2;
double m_radius;
@@ -129,15 +151,15 @@
m_radius2 = radius * radius;
}
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ protected:
+
+ virtual bool containsPt(const libTF::Pose3D::Position &p) const
{
if (fabs(p.z) > m_length2)
return false;
return p.x * p.x + p.y * p.y < m_radius2;
}
- protected:
-
double m_length;
double m_length2;
double m_radius;
@@ -164,14 +186,15 @@
m_width2 = width / 2.0;
m_height = height;
m_height2 = height / 2.0;
- }
+ }
+
+ protected:
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ virtual bool containsPt(const libTF::Pose3D::Position &p) const
{
return fabs(p.x) < m_length2 && fabs(p.y) < m_width2 && fabs(p.z) < m_height2;
}
- protected:
double m_length;
double m_width;
double m_height;
Modified: pkg/trunk/world_models/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-05 17:39:31 UTC (rev 2556)
+++ pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-05 18:18:28 UTC (rev 2557)
@@ -8,5 +8,7 @@
<depend package="xmlparam" />
<depend package="rosTF" />
<depend package="random_utils" />
+<depend package="wg_robot_description_parser" />
+<depend package="robot_motion_planning_models" />
<depend package="collision_space" />
</package>
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 17:39:31 UTC (rev 2556)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 18:18:28 UTC (rev 2557)
@@ -90,7 +90,11 @@
#include <rostools/Log.h>
#include <rosTF/rosTF.h>
#include <random_utils/random_utils.h>
+
+#include <urdf/URDF.h>
+#include <planning_models/kinematic.h>
#include <collision_space/util.h>
+
#include <deque>
#include <cmath>
@@ -107,13 +111,14 @@
advertise<PointCloudFloat32>("world_3d_map");
advertise<rostools::Log>("roserr");
- // NOTE: subscribe to stereo vision point cloud as well... when it becomes available
subscribe("cloud", inputCloud, &World3DMap::pointCloudCallback);
param("world_3d_map/max_publish_frequency", maxPublishFrequency, 0.5);
param("world_3d_map/retain_pointcloud_duration", retainPointcloudDuration, 60.0);
param("world_3d_map/retain_pointcloud_fraction", retainPointcloudFraction, 0.02);
param("world_3d_map/verbosity_level", verbose, 1);
+
+ loadRobotDescriptions();
/* create a thread that does the processing of the input data.
* and one that handles the publishing of the data */
@@ -138,8 +143,52 @@
pthread_join(*processingThread, NULL);
for (unsigned int i = 0 ; i < currentWorld.size() ; ++i)
delete currentWorld[i];
+
+ for (unsigned int i = 0 ; i < robotDescriptions.size() ; ++i)
+ {
+ delete robotDescriptions[i].urdf;
+ delete robotDescriptions[i].kmodel;
+ }
}
+ void loadRobotDescriptions(void)
+ {
+ printf("Loading robot descriptions...\n\n");
+
+ std::string description_files;
+ get_param("robotdesc_list", description_files);
+ std::stringstream sdf(description_files);
+ while (sdf.good())
+ {
+ std::string file;
+ std::string content;
+ sdf >> file;
+ printf("Loading '%s'\n", file.c_str());
+ get_param(file, content);
+ addRobotDescriptionFromData(content.c_str());
+ }
+ printf("\n\n");
+ }
+
+ void addRobotDescriptionFromData(const char *data)
+ {
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadString(data))
+ addRobotDescription(file);
+ else
+ delete file;
+ }
+
+ void addRobotDescription(robot_desc::URDF *file)
+ {
+ planning_models::KinematicModel *kmodel = new planning_models::KinematicModel();
+ kmodel->setVerbose(false);
+ kmodel->build(*file);
+
+ RobotDesc rd = { file, kmodel };
+ robotDescriptions.push_back(rd);
+ }
+
void pointCloudCallback(void)
{
/* The idea is that if processing of previous input data is
@@ -315,9 +364,17 @@
private:
- PointCloudFloat32 inputCloud; //Buffer for recieving cloud
- PointCloudFloat32 toProcess; //Buffer (size 1) for incoming cloud
- std::deque<PointCloudFloat32*> currentWorld;// Pointers to saved clouds
+ struct RobotDesc
+ {
+ robot_desc::URDF *urdf;
+ planning_models::KinematicModel *kmodel;
+ };
+
+ std::vector<RobotDesc> robotDescriptions;
+
+ PointCloudFloat32 inputCloud; //Buffer for recieving cloud
+ PointCloudFloat32 toProcess; //Buffer (size 1) for incoming cloud
+ std::deque<PointCloudFloat32*> currentWorld;// Pointers to saved clouds
rosTFClient tf;
double maxPublishFrequency;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
