Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16809] pkg/trunk
|
From: <is...@us...> - 2009-06-06 06:08:38
|
Revision: 16809
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16809&view=rev
Author: isucan
Date: 2009-06-06 06:08:33 +0000 (Sat, 06 Jun 2009)
Log Message:
-----------
new filter for removing seen parts of self
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
Added Paths:
-----------
pkg/trunk/util/new_robot_self_filter/
pkg/trunk/util/new_robot_self_filter/.build_version
pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
pkg/trunk/util/new_robot_self_filter/Makefile
pkg/trunk/util/new_robot_self_filter/mainpage.dox
pkg/trunk/util/new_robot_self_filter/manifest.xml
pkg/trunk/util/new_robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/test_filter.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-06-06 05:39:44 UTC (rev 16808)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-06-06 06:08:33 UTC (rev 16809)
@@ -93,7 +93,7 @@
public:
- KinematicStateMonitor(void) : m_tf(*ros::Node::instance(), true, ros::Duration(1))
+ KinematicStateMonitor(void) : m_tf(*ros::Node::instance(), true, ros::Duration(10))
{
m_tf.setExtrapolationLimit(ros::Duration().fromSec(10));
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-06 05:39:44 UTC (rev 16808)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-06 06:08:33 UTC (rev 16809)
@@ -39,6 +39,7 @@
#include "planning_models/kinematic.h"
#include <ros/ros.h>
+#include <ros/node.h>
#include <boost/shared_ptr.hpp>
#include <map>
@@ -81,8 +82,9 @@
};
- RobotModels(const std::string &description) : description_(description)
+ RobotModels(const std::string &description)
{
+ description_ = nh_.getNode()->mapName(description);
loadRobot();
}
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-06 05:39:44 UTC (rev 16808)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-06 06:08:33 UTC (rev 16809)
@@ -51,7 +51,6 @@
kmodel_->setVerbose(false);
kmodel_->build(*urdf_, planning_groups_);
-
// make sure the kinematic model is in its own frame
// (remove all transforms caused by planar or floating
// joints)
Added: pkg/trunk/util/new_robot_self_filter/.build_version
===================================================================
--- pkg/trunk/util/new_robot_self_filter/.build_version (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/.build_version 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1 @@
+https://ros.svn.sf.net/svnroot/personalrobots/pkg/trunk/util/new_robot_self_filter:0
Added: pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/new_robot_self_filter/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/CMakeLists.txt 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rospack(new_robot_self_filter)
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#genmsg()
+#uncomment if you have defined services
+#gensrv()
+
+#common commands for building c++ executables and libraries
+#rospack_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rospack_add_boost_directories()
+#rospack_link_boost(${PROJECT_NAME} thread)
+
+rospack_add_executable(test_filter test_filter.cpp)
+
Added: pkg/trunk/util/new_robot_self_filter/Makefile
===================================================================
--- pkg/trunk/util/new_robot_self_filter/Makefile (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/Makefile 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/util/new_robot_self_filter/mainpage.dox
===================================================================
--- pkg/trunk/util/new_robot_self_filter/mainpage.dox (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/mainpage.dox 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b new_robot_self_filter is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/util/new_robot_self_filter/manifest.xml
===================================================================
--- pkg/trunk/util/new_robot_self_filter/manifest.xml (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/manifest.xml 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,22 @@
+<package>
+ <description brief="new_robot_self_filter">
+
+ new_robot_self_filter
+
+ </description>
+ <author>Ioan Sucan</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/new_robot_self_filter</url>
+
+ <depend package="roscpp"/>
+ <depend package="tf"/>
+ <depend package="filters"/>
+ <depend package="robot_msgs"/>
+ <depend package="visualization_msgs"/>
+ <depend package="collision_space"/>
+ <depend package="planning_environment"/>
+
+</package>
+
+
Added: pkg/trunk/util/new_robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/new_robot_self_filter/self_see_filter.h (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/self_see_filter.h 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,213 @@
+/*
+ * 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, Inc. 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.
+ */
+
+#ifndef FILTERS_SELF_SEE_H_
+#define FILTERS_SELF_SEE_H_
+
+#include <filters/filter_base.h>
+#include <ros/console.h>
+#include <robot_msgs/PointCloud.h>
+#include <planning_environment/robot_models.h>
+#include <collision_space/point_inclusion.h>
+#include <tf/transform_listener.h>
+#include <string>
+#include <algorithm>
+
+namespace filters
+{
+
+/** \brief A filter to remove parts of the robot seen in a pointcloud
+ *
+ */
+
+template <typename T>
+class SelfFilter: public FilterBase <T>
+{
+
+protected:
+
+ struct SeeLink
+ {
+ std::string name;
+ collision_space::bodies::Body* body;
+ btTransform constTransf;
+ };
+
+ struct SortBodies
+ {
+ bool operator()(const SeeLink &b1, const SeeLink &b2)
+ {
+ return b1.body->computeVolume() > b2.body->computeVolume();
+ }
+ };
+
+public:
+ /** \brief Construct the filter */
+ SelfFilter(void) : rm_("robot_description"), tf_(*ros::Node::instance(), true, ros::Duration(10))
+ {
+ tf_.setExtrapolationLimit(ros::Duration().fromSec(10));
+ }
+
+
+ /** \brief Destructor to clean up
+ */
+ ~SelfFilter(void)
+ {
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ if (bodies_[i].body)
+ delete bodies_[i].body;
+ }
+
+ virtual bool configure(void)
+ {
+ std::vector<std::string> links = rm_.getSelfSeeLinks();
+ double scale = rm_.getSelfSeeScale();
+ double padd = rm_.getSelfSeePadding();
+
+ // 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)
+ {
+ SeeLink sl;
+ sl.body = collision_space::bodies::createBodyFromShape(rm_.getKinematicModel()->getLink(links[i])->shape);
+ if (sl.body)
+ {
+ sl.name = links[i];
+
+ // collision models may have an offset, in addition to what TF gives
+ // so we keep it around
+ sl.constTransf = rm_.getKinematicModel()->getLink(links[i])->constGeomTrans;
+ sl.body->setScale(scale);
+ sl.body->setPadding(padd);
+ bodies_.push_back(sl);
+ }
+ else
+ ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
+ }
+
+ if (bodies_.empty())
+ ROS_WARN("No robot links will be checked for self collision");
+
+ // put larger volume bodies first -- higher chances of containing a point
+ std::sort(bodies_.begin(), bodies_.end(), SortBodies());
+
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
+
+ return true;
+ }
+
+
+ /** \brief Update the filter and return the data seperately
+ * \param data_in T array with length width
+ * \param data_out T array with length width
+ */
+ virtual bool update(const robot_msgs::PointCloud& data_in, robot_msgs::PointCloud& data_out)
+ {
+ if (bodies_.empty())
+ data_out = data_in;
+ else
+ {
+ const unsigned int bs = bodies_.size();
+ const unsigned int np = data_in.pts.size();
+
+ // place the links in the frame of the pointcloud
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ {
+ // find the transform between the link's frame and the pointcloud frame
+ tf::Stamped<btTransform> transf;
+ tf_.lookupTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp, transf);
+
+ // set it for each body; we also include the offset specified in URDF
+ bodies_[i].body->setPose(transf * bodies_[i].constTransf);
+ }
+
+ // we now decide which points we keep
+ std::vector<bool> keep(np);
+
+#pragma omp parallel for
+ for (unsigned int i = 0 ; i < np ; ++i)
+ {
+ btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
+ bool out = true;
+ for (unsigned int j = 0 ; out && j < bs ; ++j)
+ out = !bodies_[j].body->containsPoint(pt);
+ keep[i] = out;
+ }
+
+
+ // fill in output data
+ data_out.header = data_in.header;
+
+ data_out.pts.resize(0);
+ data_out.pts.reserve(np);
+
+ data_out.chan.resize(data_in.chan.size());
+ for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
+ {
+ ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
+ data_out.chan[i].name = data_in.chan[i].name;
+ data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
+ }
+
+ for (unsigned int i = 0 ; i < np ; ++i)
+ if (keep[i])
+ {
+ data_out.pts.push_back(data_in.pts[i]);
+ for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
+ data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
+ }
+ }
+ return true;
+ }
+
+ virtual bool update(const std::vector<robot_msgs::PointCloud> & data_in, std::vector<robot_msgs::PointCloud>& data_out)
+ {
+ data_out.resize(data_in.size());
+ for (unsigned int i = 0 ; i < data_in.size() ; ++i)
+ update(data_in[i], data_out[i]);
+ return true;
+ }
+
+protected:
+
+ planning_environment::RobotModels rm_;
+
+ tf::TransformListener tf_;
+ std::vector<SeeLink> bodies_;
+
+};
+
+typedef robot_msgs::PointCloud robot_msgs_PointCloud;
+FILTERS_REGISTER_FILTER(SelfFilter, robot_msgs_PointCloud);
+
+}
+
+#endif //#ifndef FILTERS_SELF_SEE_H_
Added: pkg/trunk/util/new_robot_self_filter/test_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/test_filter.cpp (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/test_filter.cpp 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,76 @@
+/*********************************************************************
+* 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 "self_see_filter.h"
+
+class test
+{
+public:
+ test(void)
+ {
+ sf.configure();
+ sb = nh.subscribe("tilt_scan_filtered", 1, &test::myfilter, this);
+ pub = nh.advertise<robot_msgs::PointCloud>("my_point_cloud2", 10);
+
+ }
+
+ void myfilter(const robot_msgs::PointCloudConstPtr &pc)
+ {
+ robot_msgs::PointCloud out;
+ sf.update(*pc, out);
+ pub.publish(out);
+ }
+
+ ros::Subscriber sb;
+ ros::Publisher pub;
+ ros::NodeHandle nh;
+ filters::SelfFilter<robot_msgs::PointCloud> sf;
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "ttt");
+ test t;
+ ros::spin();
+
+
+ return 0;
+}
+
+
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
