Menu
▾
▴
personalrobots-commit
|
From: <is...@us...> - 2009-09-04 01:31:29
|
Revision: 23807
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23807&view=rev
Author: isucan
Date: 2009-09-04 01:31:22 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
removing draw rays debug app
Modified Paths:
--------------
pkg/trunk/motion_planning/robot_self_filter/CMakeLists.txt
Removed Paths:
-------------
pkg/trunk/motion_planning/robot_self_filter/src/draw_rays.cpp
Modified: pkg/trunk/motion_planning/robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/CMakeLists.txt 2009-09-04 01:16:47 UTC (rev 23806)
+++ pkg/trunk/motion_planning/robot_self_filter/CMakeLists.txt 2009-09-04 01:31:22 UTC (rev 23807)
@@ -24,7 +24,3 @@
rospack_add_executable(self_filter src/self_filter.cpp)
rospack_add_openmp_flags(self_filter)
target_link_libraries(self_filter robot_self_filter)
-
-rospack_add_executable(draw_rays src/draw_rays.cpp)
-rospack_add_openmp_flags(draw_rays)
-target_link_libraries(draw_rays robot_self_filter)
Deleted: pkg/trunk/motion_planning/robot_self_filter/src/draw_rays.cpp
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/src/draw_rays.cpp 2009-09-04 01:16:47 UTC (rev 23806)
+++ pkg/trunk/motion_planning/robot_self_filter/src/draw_rays.cpp 2009-09-04 01:31:22 UTC (rev 23807)
@@ -1,140 +0,0 @@
-/*********************************************************************
-* 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 */
-
-#include <ros/ros.h>
-#include <visualization_msgs/Marker.h>
-#include <sensor_msgs/PointCloud.h>
-#include <tf/message_notifier.h>
-#include <tf/transform_listener.h>
-
-class DrawRays
-{
-public:
-
- DrawRays(void) : cloudNotifier_(tf_, boost::bind(&DrawRays::cloudCallback, this, _1), "cloud_in", "laser_tilt_mount_link", 1)
- {
- id_ = 1;
- vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
- nodeHandle_.param<std::string>("~sensor_frame", sensor_frame_, "laser_tilt_mount_link");
- }
-
- ~DrawRays(void)
- {
- }
-
- void cloudCallback(const sensor_msgs::PointCloudConstPtr &cloud)
- {
- // compute the origin of the sensor in the frame of the cloud
- btVector3 sensor_pos;
- try
- {
- tf::Stamped<btTransform> transf;
- tf_.lookupTransform(cloud->header.frame_id, sensor_frame_, cloud->header.stamp, transf);
- sensor_pos = transf.getOrigin();
- }
- catch(...)
- {
- sensor_pos.setValue(0, 0, 0);
- ROS_ERROR("Unable to lookup transform from %s to %s", sensor_frame_.c_str(), cloud->header.frame_id.c_str());
- }
- for (unsigned int i = 0 ; i < cloud->points.size() ; ++i)
- sendLine(cloud->header, cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, sensor_pos.x(), sensor_pos.y(), sensor_pos.z());
- id_ = 1;
- }
-
- void sendLine(const roslib::Header &header, double x1, double y1, double z1, double x2, double y2, double z2)
- {
- visualization_msgs::Marker mk;
-
- mk.header = header;
-
- mk.ns = "draw_rays";
- mk.id = id_++;
- mk.type = visualization_msgs::Marker::ARROW;
- mk.action = visualization_msgs::Marker::ADD;
- mk.pose.position.x = 0;
- mk.pose.position.y = 0;
- mk.pose.position.z = 0;
- mk.pose.orientation.x = 0;
- mk.pose.orientation.y = 0;
- mk.pose.orientation.z = 0;
- mk.pose.orientation.w = 1.0;
-
- mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
-
- mk.color.a = 1.0;
- mk.color.r = 0.7;
- mk.color.g = 0.4;
- mk.color.b = 0.4;
-
- mk.points.resize(2);
- mk.points[0].x = x1;
- mk.points[0].y = y1;
- mk.points[0].z = z1;
-
- mk.points[1].x = x2;
- mk.points[1].y = y2;
- mk.points[1].z = z2;
-
- mk.lifetime = ros::Duration(2);
-
- vmPub_.publish(mk);
- }
-
-protected:
-
-
- ros::NodeHandle nodeHandle_;
- tf::TransformListener tf_;
- tf::MessageNotifier<sensor_msgs::PointCloud> cloudNotifier_;
- std::string sensor_frame_;
-
- ros::Publisher vmPub_;
- int id_;
-};
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "draw_rays");
-
- DrawRays t;
- ros::spin();
-
- return 0;
-}
-
-
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-04 02:15:37
|
Revision: 23808
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23808&view=rev
Author: isucan
Date: 2009-09-04 02:15:28 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
robot self filter no longer depends on planning stack
Modified Paths:
--------------
pkg/trunk/motion_planning/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/motion_planning/robot_self_filter/manifest.xml
pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp
pkg/trunk/motion_planning/robot_self_filter/src/test_filter.cpp
Modified: pkg/trunk/motion_planning/robot_self_filter/include/robot_self_filter/self_mask.h
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/include/robot_self_filter/self_mask.h 2009-09-04 01:31:22 UTC (rev 23807)
+++ pkg/trunk/motion_planning/robot_self_filter/include/robot_self_filter/self_mask.h 2009-09-04 02:15:28 UTC (rev 23808)
@@ -31,7 +31,6 @@
#define ROBOT_SELF_FILTER_SELF_MASK_
#include <sensor_msgs/PointCloud.h>
-#include <planning_environment/models/robot_models.h>
#include <geometric_shapes/bodies.h>
#include <tf/transform_listener.h>
#include <boost/bind.hpp>
@@ -82,7 +81,7 @@
public:
/** \brief Construct the filter */
- SelfMask(tf::TransformListener &tf, const std::vector<std::string> &links, double scale, double padd) : rm_("robot_description"), tf_(tf)
+ SelfMask(tf::TransformListener &tf, const std::vector<std::string> &links, double scale, double padd) : tf_(tf)
{
configure(links, scale, padd);
}
@@ -170,7 +169,6 @@
/** \brief Perform the actual mask computation. */
void maskAuxIntersection(const sensor_msgs::PointCloud& data_in, std::vector<int> &mask, const boost::function<void(const btVector3&)> &callback);
- planning_environment::RobotModels rm_;
tf::TransformListener &tf_;
ros::NodeHandle nh_;
Modified: pkg/trunk/motion_planning/robot_self_filter/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/manifest.xml 2009-09-04 01:31:22 UTC (rev 23807)
+++ pkg/trunk/motion_planning/robot_self_filter/manifest.xml 2009-09-04 02:15:28 UTC (rev 23808)
@@ -15,7 +15,9 @@
<depend package="sensor_msgs"/>
<depend package="visualization_msgs"/>
<depend package="geometric_shapes"/>
- <depend package="planning_environment"/>
+ <depend package="urdf"/>
+ <depend package="resource_retriever"/>
+ <depend package="ogre_tools"/>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrobot_self_filter" />
Modified: pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp 2009-09-04 01:31:22 UTC (rev 23807)
+++ pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp 2009-09-04 02:15:28 UTC (rev 23808)
@@ -28,9 +28,13 @@
*/
#include "robot_self_filter/self_mask.h"
+#include <urdf/model.h>
+#include <resource_retriever/retriever.h>
+#include <ogre_tools/stl_loader.h>
+#include <ros/console.h>
#include <algorithm>
+#include <sstream>
#include <climits>
-#include <ros/console.h>
void robot_self_filter::SelfMask::freeMemory(void)
{
@@ -45,23 +49,145 @@
bodies_.clear();
}
+
+namespace robot_self_filter
+{
+ static inline btTransform urdfPose2btTransform(const urdf::Pose &pose)
+ {
+ return btTransform(btQuaternion(pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w),
+ btVector3(pose.position.x, pose.position.y, pose.position.z));
+ }
+
+ static shapes::Shape* constructShape(const urdf::Geometry *geom)
+ {
+ ROS_ASSERT(geom);
+
+ shapes::Shape *result = NULL;
+ switch (geom->type)
+ {
+ case urdf::Geometry::SPHERE:
+ result = new shapes::Sphere(dynamic_cast<const urdf::Sphere*>(geom)->radius);
+ break;
+ case urdf::Geometry::BOX:
+ {
+ urdf::Vector3 dim = dynamic_cast<const urdf::Box*>(geom)->dim;
+ result = new shapes::Box(dim.x, dim.y, dim.z);
+ }
+ break;
+ case urdf::Geometry::CYLINDER:
+ result = new shapes::Cylinder(dynamic_cast<const urdf::Cylinder*>(geom)->radius,
+ dynamic_cast<const urdf::Cylinder*>(geom)->length);
+ break;
+ case urdf::Geometry::MESH:
+ {
+ const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
+ if (!mesh->filename.empty())
+ {
+ resource_retriever::Retriever retriever;
+ resource_retriever::MemoryResource res;
+ bool ok = true;
+
+ try
+ {
+ res = retriever.get(mesh->filename);
+ }
+ catch (resource_retriever::Exception& e)
+ {
+ ROS_ERROR("%s", e.what());
+ ok = false;
+ }
+
+ if (ok)
+ {
+ if (res.size == 0)
+ ROS_WARN("Retrieved empty mesh for resource '%s'", mesh->filename.c_str());
+ else
+ {
+ ogre_tools::STLLoader loader;
+ if (loader.load(res.data.get()))
+ {
+ std::vector<btVector3> triangles;
+ for (unsigned int i = 0 ; i < loader.triangles_.size() ; ++i)
+ {
+ triangles.push_back(btVector3(loader.triangles_[i].vertices_[0].x, loader.triangles_[i].vertices_[0].y, loader.triangles_[i].vertices_[0].z));
+ triangles.push_back(btVector3(loader.triangles_[i].vertices_[1].x, loader.triangles_[i].vertices_[1].y, loader.triangles_[i].vertices_[1].z));
+ triangles.push_back(btVector3(loader.triangles_[i].vertices_[2].x, loader.triangles_[i].vertices_[2].y, loader.triangles_[i].vertices_[2].z));
+ }
+ result = shapes::createMeshFromVertices(triangles);
+ }
+ else
+ ROS_ERROR("Failed to load mesh '%s'", mesh->filename.c_str());
+ }
+ }
+ }
+ else
+ ROS_WARN("Empty mesh filename");
+ }
+
+ break;
+ default:
+ ROS_ERROR("Unknown geometry type: %d", (int)geom->type);
+ break;
+ }
+
+ return result;
+ }
+}
+
bool robot_self_filter::SelfMask::configure(const std::vector<std::string> &links, double scale, double padd)
{
// in case configure was called before, we free the memory
freeMemory();
sensor_pos_.setValue(0, 0, 0);
+ std::string content;
+ boost::shared_ptr<urdf::Model> urdfModel;
+
+ if (nh_.getParam("robot_description", content))
+ {
+ urdfModel = boost::shared_ptr<urdf::Model>(new urdf::Model());
+ if (!urdfModel->initString(content))
+ {
+ ROS_ERROR("Unable to parse URDF description!");
+ return false;
+ }
+ }
+ else
+ {
+ ROS_ERROR("Robot model not found! Did you remap 'robot_description'?");
+ return false;
+ }
+
+ std::stringstream missing;
+
// from the geometric model, find the shape of each link of interest
// and create a body from it, one that knows about poses and can
// check for point inclusion
for (unsigned int i = 0 ; i < links.size() ; ++i)
{
- planning_models::KinematicModel::Link *link = rm_.getKinematicModel()->getLink(links[i]);
+ const urdf::Link *link = urdfModel->getLink(links[i]).get();
if (!link)
+ {
+ missing << " " << links[i];
continue;
+ }
+ if (!(link->collision && link->collision->geometry))
+ {
+ ROS_WARN("No collision geometry specified for link '%s'", links[i].c_str());
+ continue;
+ }
+
+ shapes::Shape *shape = constructShape(link->collision->geometry.get());
+
+ if (!shape)
+ {
+ ROS_ERROR("Unable to construct collision shape for link '%s'", links[i].c_str());
+ continue;
+ }
+
SeeLink sl;
- sl.body = bodies::createBodyFromShape(link->shape);
+ sl.body = bodies::createBodyFromShape(shape);
if (sl.body)
{
@@ -69,19 +195,25 @@
// collision models may have an offset, in addition to what TF gives
// so we keep it around
- sl.constTransf = link->constGeomTrans;
+ sl.constTransf = urdfPose2btTransform(link->collision->origin);
+
sl.body->setScale(scale);
sl.body->setPadding(padd);
sl.volume = sl.body->computeVolume();
- sl.unscaledBody = bodies::createBodyFromShape(link->shape);
+ sl.unscaledBody = bodies::createBodyFromShape(shape);
bodies_.push_back(sl);
}
else
ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
+
+ delete shape;
}
+ if (missing.str().size() > 0)
+ ROS_WARN("Some links were included for self mask but they do not exist in the model:%s", missing.str().c_str());
+
if (bodies_.empty())
- ROS_WARN("No robot links will be checked for self collision");
+ ROS_WARN("No robot links will be checked for self mask");
// put larger volume bodies first -- higher chances of containing a point
std::sort(bodies_.begin(), bodies_.end(), SortBodies());
Modified: pkg/trunk/motion_planning/robot_self_filter/src/test_filter.cpp
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/src/test_filter.cpp 2009-09-04 01:31:22 UTC (rev 23807)
+++ pkg/trunk/motion_planning/robot_self_filter/src/test_filter.cpp 2009-09-04 02:15:28 UTC (rev 23808)
@@ -42,7 +42,7 @@
{
public:
- TestSelfFilter(void) : rm_("robot_description")
+ TestSelfFilter(void)
{
id_ = 1;
vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
@@ -146,7 +146,6 @@
tf::TransformListener tf_;
robot_self_filter::SelfMask *sf_;
- planning_environment::RobotModels rm_;
ros::Publisher vmPub_;
ros::NodeHandle nodeHandle_;
int id_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X