Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19975] pkg/trunk
|
From: <is...@us...> - 2009-07-29 04:31:29
|
Revision: 19975
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19975&view=rev
Author: isucan
Date: 2009-07-29 04:31:22 +0000 (Wed, 29 Jul 2009)
Log Message:
-----------
implemented collision_map_self_occ based on the raytracing abilities added earlier this week; switched perception to use collision_map_self_occ instead of collision_map_occ;
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/CMakeLists.txt
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/robot_self_filter/src/self_mask.cpp
Added Paths:
-----------
pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/
pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_self_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser+stereo-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/narrow_stereoproc.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/stereo-perception.launch
Removed Paths:
-------------
pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch
pkg/trunk/sandbox/3dnav_pr2/launch/stereo-perception.launch
Modified: pkg/trunk/mapping/collision_map/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-07-29 04:31:22 UTC (rev 19975)
@@ -82,3 +82,7 @@
rospack_add_executable(collision_map_occ_node src/collision_map_occ.cpp)
rospack_add_openmp_flags(collision_map_occ_node)
rospack_link_boost(collision_map_occ_node thread)
+
+rospack_add_executable(collision_map_self_occ_node src/collision_map_self_occ.cpp)
+rospack_add_openmp_flags(collision_map_self_occ_node)
+rospack_link_boost(collision_map_self_occ_node thread)
Modified: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-29 04:31:22 UTC (rev 19975)
@@ -828,7 +828,7 @@
int main (int argc, char** argv)
{
- ros::init(argc, argv, "collision_map_occ");
+ ros::init(argc, argv, "collision_map_occ", ros::init_options::AnonymousName);
CollisionMapperOcc cm;
Copied: pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp (from rev 19967, pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp)
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp (rev 0)
+++ pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-07-29 04:31:22 UTC (rev 19975)
@@ -0,0 +1,411 @@
+/*********************************************************************
+* 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 <robot_msgs/PointCloud.h>
+#include <mapping_msgs/CollisionMap.h>
+#include <tf/transform_listener.h>
+#include <tf/message_notifier.h>
+#include <robot_self_filter/self_mask.h>
+#include <boost/thread/mutex.hpp>
+#include <boost/bind.hpp>
+#include <vector>
+#include <algorithm>
+#include <set>
+#include <iterator>
+#include <cstdlib>
+
+class CollisionMapperOcc
+{
+public:
+
+ CollisionMapperOcc(void) : sf_(tf_)
+ {
+ // read ROS params
+ loadParams();
+
+ // advertise our topics: full map and updates
+ cmapPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ", 1);
+ cmapUpdPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_update", 1);
+ if (publishOcclusion_)
+ occPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_occlusion", 1);
+
+ // create a message notifier (and enable subscription) for both the full map and for the updates
+ mnCloud_ = new tf::MessageNotifier<robot_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudCallback, this, _1), "cloud_in", "", 1);
+ mnCloudIncremental_ = new tf::MessageNotifier<robot_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudIncrementalCallback, this, _1), "cloud_incremental_in", "", 1);
+
+ // configure the self mask and the message notifier
+ std::vector<std::string> frames;
+ sf_.getLinkNames(frames);
+ if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end())
+ frames.push_back(robotFrame_);
+ mnCloud_->setTargetFrame(frames);
+ mnCloudIncremental_->setTargetFrame(frames);
+ }
+
+ ~CollisionMapperOcc(void)
+ {
+ delete mnCloud_;
+ delete mnCloudIncremental_;
+ }
+
+ void run(void)
+ {
+ if (bi_.sensor_frame.empty())
+ ROS_ERROR("No sensor frame specified. Cannot perform raytracing");
+ else
+ ros::spin();
+ }
+
+private:
+
+ struct CollisionPoint
+ {
+ CollisionPoint(void) {}
+ CollisionPoint(int _x, int _y, int _z) : x(_x), y(_y), z(_z) {}
+
+ int x, y, z;
+ };
+
+ // define an order on points
+ struct CollisionPointOrder
+ {
+ bool operator()(const CollisionPoint &a, const CollisionPoint &b) const
+ {
+ if (a.x < b.x)
+ return true;
+ if (a.x > b.x)
+ return false;
+ if (a.y < b.y)
+ return true;
+ if (a.y > b.y)
+ return false;
+ return a.z < b.z;
+ }
+ };
+
+ typedef std::set<CollisionPoint, CollisionPointOrder> CMap;
+
+ // parameters & precomputed values for the box that represents the collision map
+ // around the robot
+ struct BoxInfo
+ {
+ double dimensionX, dimensionY, dimensionZ;
+ double originX, originY, originZ;
+ std::string sensor_frame;
+ double resolution;
+ double real_minX, real_minY, real_minZ;
+ double real_maxX, real_maxY, real_maxZ;
+ };
+
+ void loadParams(void)
+ {
+ // a frame that does not move with the robot
+ nh_.param<std::string>("~fixed_frame", fixedFrame_, "odom");
+
+ // a frame that moves with the robot
+ nh_.param<std::string>("~robot_frame", robotFrame_, "base_link");
+
+ // bounds of collision map in robot frame
+ nh_.param<double>("~dimension_x", bi_.dimensionX, 1.0);
+ nh_.param<double>("~dimension_y", bi_.dimensionY, 1.5);
+ nh_.param<double>("~dimension_z", bi_.dimensionZ, 2.0);
+
+ // origin of collision map in the robot frame
+ nh_.param<double>("~origin_x", bi_.originX, 1.1);
+ nh_.param<double>("~origin_y", bi_.originY, 0.0);
+ nh_.param<double>("~origin_z", bi_.originZ, 0.0);
+
+ // sensor frame
+ nh_.param<std::string>("~sensor_frame", bi_.sensor_frame, std::string());
+
+ // resolution
+ nh_.param<double>("~resolution", bi_.resolution, 0.015);
+
+ ROS_INFO("Maintaining occlusion map in frame '%s', with origin at (%f, %f, %f) and dimension (%f, %f, %f), resolution of %f; "
+ "sensor is in frame '%s', fixed fame is '%s'.",
+ robotFrame_.c_str(), bi_.dimensionX, bi_.dimensionY, bi_.dimensionZ, bi_.originX, bi_.originY, bi_.originZ,
+ bi_.resolution, bi_.sensor_frame.c_str(), fixedFrame_.c_str());
+
+ nh_.param<bool>("~publish_occlusion", publishOcclusion_, false);
+
+ // compute some useful values
+ bi_.real_minX = -bi_.dimensionX + bi_.originX;
+ bi_.real_maxX = bi_.dimensionX + bi_.originX;
+ bi_.real_minY = -bi_.dimensionY + bi_.originY;
+ bi_.real_maxY = bi_.dimensionY + bi_.originY;
+ bi_.real_minZ = -bi_.dimensionZ + bi_.originZ;
+ bi_.real_maxZ = bi_.dimensionZ + bi_.originZ;
+ }
+
+ void cloudIncrementalCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ {
+ if (!mapProcessing_.try_lock())
+ return;
+
+ ROS_DEBUG("Got pointcloud update that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
+
+ robot_msgs::PointCloud out;
+ ros::WallTime tm = ros::WallTime::now();
+
+ // transform the pointcloud to the robot frame
+ // since we need the points in this frame (around the robot)
+ // to compute the collision map
+ tf_.transformPointCloud(robotFrame_, *cloud, out);
+
+ CMap obstacles;
+ constructCollisionMap(out, obstacles);
+
+ CMap diff;
+ set_difference(obstacles.begin(), obstacles.end(), currentMap_.begin(), currentMap_.end(),
+ std::inserter(diff, diff.begin()), CollisionPointOrder());
+ mapProcessing_.unlock();
+
+ if (!diff.empty())
+ publishCollisionMap(diff, out.header, cmapUpdPublisher_);
+ }
+
+ void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ {
+ ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
+
+ mapProcessing_.lock();
+
+ robot_msgs::PointCloud out;
+ ros::WallTime tm = ros::WallTime::now();
+
+ CMap obstacles;
+
+ // transform the pointcloud to the robot frame
+ // since we need the points in this frame (around the robot)
+ // to compute the collision map
+ tf_.transformPointCloud(robotFrame_, *cloud, out);
+ constructCollisionMap(out, obstacles);
+
+ // try to transform the previous map (if it exists) to the new frame
+ if (!currentMap_.empty())
+ if (!transformMap(currentMap_, header_, out.header))
+ currentMap_.clear();
+ header_ = out.header;
+
+ // update map
+ updateMap(obstacles);
+
+ double sec = (ros::WallTime::now() - tm).toSec();
+ ROS_INFO("Updated collision map with %d points at %f Hz", currentMap_.size(), 1.0/sec);
+
+ publishCollisionMap(currentMap_, header_, cmapPublisher_);
+ mapProcessing_.unlock();
+ }
+
+ void updateMap(CMap &obstacles)
+ {
+ if (currentMap_.empty())
+ currentMap_ = obstacles;
+ else
+ {
+ CMap diff;
+
+ // find the points from the old map that are no longer visible
+ set_difference(currentMap_.begin(), currentMap_.end(), obstacles.begin(), obstacles.end(),
+ std::inserter(diff, diff.begin()), CollisionPointOrder());
+
+ // the current map will at least contain the new info
+ currentMap_ = obstacles;
+
+ // find out which of these points are now occluded
+ sf_.assumeFrame(header_, bi_.sensor_frame);
+
+ // OpenMP need an int as the lookup variable, but for set,
+ // this is not possible, so we copy to a vector
+ int n = diff.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(diff.begin(), diff.end(), pts.begin());
+
+ // add points occluded by self
+ if (publishOcclusion_)
+ {
+ CMap keep;
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ btVector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
+ ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
+ ((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
+ if (sf_.getMaskIntersection(p) == robot_self_filter::SHADOW)
+ {
+#pragma omp critical
+ {
+ keep.insert(pts[i]);
+ currentMap_.insert(pts[i]);
+ }
+ }
+ }
+
+ publishCollisionMap(keep, header_, occPublisher_);
+ }
+ else
+ {
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ btVector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
+ ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
+ ((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
+ if (sf_.getMaskIntersection(p) == robot_self_filter::SHADOW)
+ {
+#pragma omp critical
+ {
+ currentMap_.insert(pts[i]);
+ }
+ }
+ }
+
+ }
+
+ }
+ }
+
+ bool transformMap(CMap &map, const roslib::Header &from, const roslib::Header &to)
+ {
+ if (tf_.canTransform(to.frame_id, to.stamp, from.frame_id, from.stamp, fixedFrame_))
+ {
+ tf::Stamped<btTransform> transf;
+ tf_.lookupTransform(to.frame_id, to.stamp, from.frame_id, from.stamp, fixedFrame_, transf);
+
+ // copy data to temporary location
+ const int n = map.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(map.begin(), map.end(), pts.begin());
+ map.clear();
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ btVector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
+ ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
+ ((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
+ p = transf * p;
+ if (p.x() > bi_.real_minX && p.x() < bi_.real_maxX && p.y() > bi_.real_minY && p.y() < bi_.real_maxY && p.z() > bi_.real_minZ && p.z() < bi_.real_maxZ)
+ {
+ CollisionPoint c((int)(0.5 + (p.x() - bi_.originX) / bi_.resolution),
+ (int)(0.5 + (p.y() - bi_.originY) / bi_.resolution),
+ (int)(0.5 + (p.z() - bi_.originZ) / bi_.resolution));
+ map.insert(c);
+ }
+ }
+
+ return true;
+ }
+ else
+ {
+ ROS_WARN("Unable to transform previous collision map into new frame");
+ return false;
+ }
+ }
+
+ /** Construct an axis-aligned collision map from a point cloud assumed to be in the robot frame */
+ void constructCollisionMap(const robot_msgs::PointCloud &cloud, CMap &map)
+ {
+ const unsigned int n = cloud.pts.size();
+ CollisionPoint c;
+
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ const robot_msgs::Point32 &p = cloud.pts[i];
+ if (p.x > bi_.real_minX && p.x < bi_.real_maxX && p.y > bi_.real_minY && p.y < bi_.real_maxY && p.z > bi_.real_minZ && p.z < bi_.real_maxZ)
+ {
+ c.x = (int)(0.5 + (p.x - bi_.originX) / bi_.resolution);
+ c.y = (int)(0.5 + (p.y - bi_.originY) / bi_.resolution);
+ c.z = (int)(0.5 + (p.z - bi_.originZ) / bi_.resolution);
+ map.insert(c);
+ }
+ }
+ }
+
+ void publishCollisionMap(const CMap &map, const roslib::Header &header, ros::Publisher &pub) const
+ {
+ mapping_msgs::CollisionMap cmap;
+ cmap.header = header;
+ const unsigned int ms = map.size();
+
+ for (CMap::const_iterator it = map.begin() ; it != map.end() ; ++it)
+ {
+ const CollisionPoint &cp = *it;
+ mapping_msgs::OrientedBoundingBox box;
+ box.extents.x = box.extents.y = box.extents.z = bi_.resolution;
+ box.axis.x = box.axis.y = 0.0; box.axis.z = 1.0;
+ box.angle = 0.0;
+ box.center.x = cp.x * bi_.resolution + bi_.originX;
+ box.center.y = cp.y * bi_.resolution + bi_.originY;
+ box.center.z = cp.z * bi_.resolution + bi_.originZ;
+ cmap.boxes.push_back(box);
+ }
+ pub.publish(cmap);
+
+ ROS_DEBUG("Published collision map with %u boxes", ms);
+ }
+
+ tf::TransformListener tf_;
+ robot_self_filter::SelfMask sf_;
+ tf::MessageNotifier<robot_msgs::PointCloud> *mnCloud_;
+ tf::MessageNotifier<robot_msgs::PointCloud> *mnCloudIncremental_;
+ ros::NodeHandle nh_;
+ ros::Publisher cmapPublisher_;
+ ros::Publisher cmapUpdPublisher_;
+ ros::Publisher occPublisher_;
+ roslib::Header header_;
+ bool publishOcclusion_;
+
+ boost::mutex mapProcessing_;
+ CMap currentMap_;
+
+ BoxInfo bi_;
+ std::string fixedFrame_;
+ std::string robotFrame_;
+
+};
+
+int main (int argc, char** argv)
+{
+ ros::init(argc, argv, "collision_map_self_occ", ros::init_options::AnonymousName);
+
+ CollisionMapperOcc cm;
+ cm.run();
+
+ return 0;
+}
Property changes on: pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/mapping/collision_map/src/collision_map_occ.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -1,45 +0,0 @@
-<launch>
- <node pkg="collision_map" type="collision_map_occ_node" name="collision_map_occ" respawn="true" output="screen">
- <remap from="cloud_in" to="full_cloud_annotated" />
- <remap from="cloud_incremental_in" to="tilt_scan_cloud_annotated" />
-
- <!-- we do not want a separate map with occlusions alone -->
- <param name="publish_occlusion" type="bool" value="false" />
-
- <!-- we do not want to assume initially occluded space (when we start the robot) is in collision -->
- <param name="mark_self_occlusion" type="bool" value="false" />
-
- <!-- tell the collision map there may be an annotation channel telling which points are on the robot -->
- <param name="cloud_annotation" type="string" value="on_self" />
-
- <!-- a frame that does not change as the robot moves -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
- <param name="fixed_frame" type="string" value="base_link" />
-
- <!-- define a box of size 2x3x4 around (1.1, 0, 0) in the robot frame -->
- <param name="robot_frame" type="string" value="base_link" />
-
- <param name="origin_x" type="double" value="1.1" />
- <param name="origin_y" type="double" value="0.0" />
- <param name="origin_z" type="double" value="0.0" />
-
- <param name="dimension_x" type="double" value="1.0" />
- <param name="dimension_y" type="double" value="1.5" />
- <param name="dimension_z" type="double" value="2.0" />
-
- <!-- set the resolution (1.0 cm) -->
- <param name="resolution" type="double" value="0.01" />
-
- <!-- the amount of time to remember an occluded point for -->
- <param name="keep_occluded" type="double" value="30.0" />
-
- <!-- when occluded obstacles are raytraced, keep boxes from occluded space within a given radius -->
- <param name="radius" type="int" value="5" />
-
- <!-- define sensor location in robot frame -->
- <param name="sensor_x" type="double" value="0.05" />
- <param name="sensor_y" type="double" value="0.0" />
- <param name="sensor_z" type="double" value="1.0" />
-
- </node>
-</launch>
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -1,97 +0,0 @@
-<launch>
-
- <!-- send additional description parameters -->
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
-
- <!-- start controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 8 .75 .25" />
-
- <!-- convert laser scan to pointcloud -->
- <node pkg="laser_scan" type="scan_to_cloud" output="screen">
- <param name="scan_topic" type="string" value="tilt_scan" />
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="high_fidelity" type="bool" value="true" />
- <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
- </node>
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="tilt_scan_cloud" />
- <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.05" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
- <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
- </node>
-
- <!-- assemble pointcloud into a full world view -->
- <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
- <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
- <param name="tf_cache_time_secs" type="double" value="10.0" />
- <param name="tf_tolerance_secs" type="double" value="0.0" />
- <param name="max_scans" type="int" value="400" />
- <param name="ignore_laser_skew" type="bool" value="true" />
- <param name="fixed_frame" type="string" value="torso_lift_link" />
- <param name="downsample_factor" type="int" value="2" />
- </node>
-
- <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
- <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
- <remap from="full_cloud" to="full_laser_cloud_annotated" />
- </node>
-
- <include file="$(find 3dnav_pr2)/launch/narrow_stereoproc.launch" />
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="/narrow_stereo/cloud" />
- <remap from="cloud_out" to="stereo_cloud_without_known_objects" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="stereo_cloud_without_known_objects" />
- <remap from="cloud_out" to="full_stereo_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- </node>
-
- <node pkg="point_cloud_assembler" type="merge_clouds" output="screen">
-
- <!-- first input cloud -->
- <remap from="cloud_in1" to="full_stereo_cloud_annotated"/>
-
- <!-- second input cloud -->
- <remap from="cloud_in2" to="full_laser_cloud_annotated"/>
-
- <!-- output cloud -->
- <remap from="cloud_out" to="full_cloud_annotated"/>
-
- <!-- the frame in which the output cloud should be published; this parameter must ALWAYS be specified -->
- <param name="output_frame" type="string" value="base_link"/>
- </node>
-
-
- <!-- start collision map -->
- <include file="$(find 3dnav_pr2)/launch/collision_map_occ.launch" />
-
-</launch>
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -1,61 +0,0 @@
-<launch>
-
- <!-- send additional description parameters -->
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
-
- <!-- start controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 10 .75 .25" />
-
- <!-- convert laser scan to pointcloud -->
- <node pkg="laser_scan" type="scan_to_cloud" output="screen">
- <param name="scan_topic" type="string" value="tilt_scan" />
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="high_fidelity" type="bool" value="true" />
- <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
- </node>
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="tilt_scan_cloud" />
- <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.05" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
- <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- <!-- we also want to remove shadow points -->
- <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
- </node>
-
- <!-- assemble pointcloud into a full world view -->
- <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
- <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
- <param name="tf_cache_time_secs" type="double" value="10.0" />
- <param name="tf_tolerance_secs" type="double" value="0.0" />
- <param name="max_scans" type="int" value="400" />
- <param name="ignore_laser_skew" type="bool" value="true" />
- <param name="fixed_frame" type="string" value="torso_lift_link" />
- <param name="downsample_factor" type="int" value="2" />
- </node>
-
- <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
- <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
- <remap from="full_cloud" to="full_cloud_annotated" />
- </node>
-
- <!-- start collision map -->
- <include file="$(find 3dnav_pr2)/launch/collision_map_occ.launch" />
-
-</launch>
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -1,41 +0,0 @@
-<launch>
- <!-- videre_mode should be one of the following:
- disparity: Disparity and rectification done on chip.
- Provides: Disparity and left mono image
- disparity_raw: Disparity done on chip (debayer and rectification in software).
- Provides: disparity and left color image.
- none: No stereo on chip (all processing done in software).
- Provides: all 3 images available
- rectified: Rectification on chip
- Provides: all 3 images available but no color
- -->
- <!-- stereo processing parameters
- texture_thresh: Threshold for removing bad disparities based on texture
- Default value: 30
- unique_thresh: Threshold for removing bad disparities based on ambiguity
- Default value: 36
- speckle_diff: Threshold for region-growing (1/16 pixel disparity)
- Default value: 10
- speckle_size: Threshold for disparity region size (pixels)
- Default value: 100
- horopter: X offset for close-in stereo (pixels)
- Default value: 0
- corr_size: Correlation window size (pixels)
- Default value: 15
- num_disp: Number of disparities (pixels)
- Default value: 64
- -->
- <group ns="narrow_stereo">
- <node pkg="stereo_image_proc" type="stereoproc" respawn="false" output="screen" name="stereoproc">
- <param name="videre_mode" type="str" value="none"/>
- <param name="do_colorize" type="bool" value="True"/>
- <param name="do_rectify" type="bool" value="True"/>
- <param name="do_stereo" type="bool" value="True"/>
- <param name="do_calc_points" type="bool" value="True"/>
- <param name="do_keep_coords" type="bool" value="True"/>
- <param name="num_disp" type="int" value="128"/>
- <param name="corr_size" type="int" value="7"/>
- </node>
- </group>
-</launch>
-
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_occ.launch (from rev 19967, pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_occ.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_occ.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -0,0 +1,45 @@
+<launch>
+ <node pkg="collision_map" type="collision_map_occ_node" respawn="true" output="screen">
+ <remap from="cloud_in" to="full_cloud_annotated" />
+ <remap from="cloud_incremental_in" to="tilt_scan_cloud_annotated" />
+
+ <!-- we do not want a separate map with occlusions alone -->
+ <param name="publish_occlusion" type="bool" value="false" />
+
+ <!-- we do not want to assume initially occluded space (when we start the robot) is in collision -->
+ <param name="mark_self_occlusion" type="bool" value="false" />
+
+ <!-- tell the collision map there may be an annotation channel telling which points are on the robot -->
+ <param name="cloud_annotation" type="string" value="on_self" />
+
+ <!-- a frame that does not change as the robot moves -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+ <param name="fixed_frame" type="string" value="base_link" />
+
+ <!-- define a box of size 2x3x4 around (1.1, 0, 0) in the robot frame -->
+ <param name="robot_frame" type="string" value="base_link" />
+
+ <param name="origin_x" type="double" value="1.1" />
+ <param name="origin_y" type="double" value="0.0" />
+ <param name="origin_z" type="double" value="0.0" />
+
+ <param name="dimension_x" type="double" value="1.0" />
+ <param name="dimension_y" type="double" value="1.5" />
+ <param name="dimension_z" type="double" value="2.0" />
+
+ <!-- set the resolution (1.0 cm) -->
+ <param name="resolution" type="double" value="0.01" />
+
+ <!-- the amount of time to remember an occluded point for -->
+ <param name="keep_occluded" type="double" value="30.0" />
+
+ <!-- when occluded obstacles are raytraced, keep boxes from occluded space within a given radius -->
+ <param name="radius" type="int" value="5" />
+
+ <!-- define sensor location in robot frame -->
+ <param name="sensor_x" type="double" value="0.05" />
+ <param name="sensor_y" type="double" value="0.0" />
+ <param name="sensor_z" type="double" value="1.0" />
+
+ </node>
+</launch>
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_occ.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/collision_map_occ.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_self_occ.launch (from rev 19967, pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_self_occ.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_self_occ.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -0,0 +1,31 @@
+<launch>
+ <node pkg="collision_map" type="collision_map_self_occ_node" respawn="true" output="screen">
+ <remap from="cloud_in" to="full_cloud_annotated" />
+ <remap from="cloud_incremental_in" to="tilt_scan_cloud_annotated" />
+
+ <!-- we do not want a separate map with occlusions alone -->
+ <param name="publish_occlusion" type="bool" value="true" />
+
+ <!-- a frame that does not change as the robot moves -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+ <param name="fixed_frame" type="string" value="base_link" />
+
+ <!-- define a box of size 2x3x4 around (1.1, 0, 0) in the robot frame -->
+ <param name="robot_frame" type="string" value="base_link" />
+
+ <param name="origin_x" type="double" value="1.1" />
+ <param name="origin_y" type="double" value="0.0" />
+ <param name="origin_z" type="double" value="0.0" />
+
+ <param name="dimension_x" type="double" value="1.0" />
+ <param name="dimension_y" type="double" value="1.5" />
+ <param name="dimension_z" type="double" value="2.0" />
+
+ <!-- set the resolution (1.0 cm) -->
+ <param name="resolution" type="double" value="0.01" />
+
+ <!-- define sensor frame -->
+ <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
+
+ </node>
+</launch>
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_self_occ.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/collision_map_occ.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser+stereo-perception.launch (from rev 19967, pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser+stereo-perception.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser+stereo-perception.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -0,0 +1,97 @@
+<launch>
+
+ <!-- send additional description parameters -->
+ <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+
+ <!-- start controller -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 8 .75 .25" />
+
+ <!-- convert laser scan to pointcloud -->
+ <node pkg="laser_scan" type="scan_to_cloud" output="screen">
+ <param name="scan_topic" type="string" value="tilt_scan" />
+ <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
+ <param name="high_fidelity" type="bool" value="true" />
+ <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
+ </node>
+
+ <!-- remove points corresponding to known objects -->
+ <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <remap from="robot_description" to="robot_description" />
+
+<!-- define a frame that stays fixed for the known objects -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+
+ <remap from="cloud_in" to="tilt_scan_cloud" />
+ <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
+ <param name="object_scale" type="double" value="1.0" />
+ <param name="object_padd" type="double" value="0.05" />
+ </node>
+
+ <!-- add a channel that marks points that are on the robot -->
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robot_description" />
+ <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
+ <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
+ <!-- <param name="annotate" type="string" value="on_self" /> -->
+ <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
+ </node>
+
+ <!-- assemble pointcloud into a full world view -->
+ <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
+ <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="tf_tolerance_secs" type="double" value="0.0" />
+ <param name="max_scans" type="int" value="400" />
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ <param name="fixed_frame" type="string" value="torso_lift_link" />
+ <param name="downsample_factor" type="int" value="2" />
+ </node>
+
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
+ <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
+ <remap from="full_cloud" to="full_laser_cloud_annotated" />
+ </node>
+
+ <include file="$(find 3dnav_pr2)/launch/perception/narrow_stereoproc.launch" />
+
+ <!-- remove points corresponding to known objects -->
+ <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <remap from="robot_description" to="robot_description" />
+
+<!-- define a frame that stays fixed for the known objects -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+
+ <remap from="cloud_in" to="/narrow_stereo/cloud" />
+ <remap from="cloud_out" to="stereo_cloud_without_known_objects" />
+ </node>
+
+ <!-- add a channel that marks points that are on the robot -->
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robot_description" />
+ <remap from="cloud_in" to="stereo_cloud_without_known_objects" />
+ <remap from="cloud_out" to="full_stereo_cloud_annotated" />
+ <!-- <param name="annotate" type="string" value="on_self" /> -->
+ </node>
+
+ <node pkg="point_cloud_assembler" type="merge_clouds" output="screen">
+
+ <!-- first input cloud -->
+ <remap from="cloud_in1" to="full_stereo_cloud_annotated"/>
+
+ <!-- second input cloud -->
+ <remap from="cloud_in2" to="full_laser_cloud_annotated"/>
+
+ <!-- output cloud -->
+ <remap from="cloud_out" to="full_cloud_annotated"/>
+
+ <!-- the frame in which the output cloud should be published; this parameter must ALWAYS be specified -->
+ <param name="output_frame" type="string" value="base_link"/>
+ </node>
+
+
+ <!-- start collision map -->
+ <include file="$(find 3dnav_pr2)/launch/perception/collision_map_self_occ.launch" />
+
+</launch>
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser+stereo-perception.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch (from rev 19967, pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -0,0 +1,61 @@
+<launch>
+
+ <!-- send additional description parameters -->
+ <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+
+ <!-- start controller -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 10 .75 .25" />
+
+ <!-- convert laser scan to pointcloud -->
+ <node pkg="laser_scan" type="scan_to_cloud" output="screen">
+ <param name="scan_topic" type="string" value="tilt_scan" />
+ <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
+ <param name="high_fidelity" type="bool" value="true" />
+ <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
+ </node>
+
+ <!-- remove points corresponding to known objects -->
+ <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <remap from="robot_description" to="robot_description" />
+
+<!-- define a frame that stays fixed for the known objects -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+
+ <remap from="cloud_in" to="tilt_scan_cloud" />
+ <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
+ <param name="object_scale" type="double" value="1.0" />
+ <param name="object_padd" type="double" value="0.05" />
+ </node>
+
+ <!-- add a channel that marks points that are on the robot -->
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robot_description" />
+ <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
+ <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
+<!-- <param name="annotate" type="string" value="on_self" /> -->
+ <!-- we also want to remove shadow points -->
+ <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
+ </node>
+
+ <!-- assemble pointcloud into a full world view -->
+ <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
+ <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="tf_tolerance_secs" type="double" value="0.0" />
+ <param name="max_scans" type="int" value="400" />
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ <param name="fixed_frame" type="string" value="torso_lift_link" />
+ <param name="downsample_factor" type="int" value="2" />
+ </node>
+
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
+ <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
+ <remap from="full_cloud" to="full_cloud_annotated" />
+ </node>
+
+ <!-- start collision map -->
+ <include file="$(find 3dnav_pr2)/launch/perception/collision_map_self_occ.launch" />
+
+</launch>
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/laser-perception.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/perception/narrow_stereoproc.launch (from rev 19967, pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/narrow_stereoproc.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/narrow_stereoproc.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -0,0 +1,41 @@
+<launch>
+ <!-- videre_mode should be one of the following:
+ disparity: Disparity and rectification done on chip.
+ Provides: Disparity and left mono image
+ disparity_raw: Disparity done on chip (debayer and rectification in software).
+ Provides: disparity and left color image.
+ none: No stereo on chip (all processing done in software).
+ Provides: all 3 images available
+ rectified: Rectification on chip
+ Provides: all 3 images available but no color
+ -->
+ <!-- stereo processing parameters
+ texture_thresh: Threshold for removing bad disparities based on texture
+ Default value: 30
+ unique_thresh: Threshold for removing bad disparities based on ambiguity
+ Default value: 36
+ speckle_diff: Threshold for region-growing (1/16 pixel disparity)
+ Default value: 10
+ speckle_size: Threshold for disparity region size (pixels)
+ Default value: 100
+ horopter: X offset for close-in stereo (pixels)
+ Default value: 0
+ corr_size: Correlation window size (pixels)
+ Default value: 15
+ num_disp: Number of disparities (pixels)
+ Default value: 64
+ -->
+ <group ns="narrow_stereo">
+ <node pkg="stereo_image_proc" type="stereoproc" respawn="false" output="screen" name="stereoproc">
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="do_colorize" type="bool" value="True"/>
+ <param name="do_rectify" type="bool" value="True"/>
+ <param name="do_stereo" type="bool" value="True"/>
+ <param name="do_calc_points" type="bool" value="True"/>
+ <param name="do_keep_coords" type="bool" value="True"/>
+ <param name="num_disp" type="int" value="128"/>
+ <param name="corr_size" type="int" value="7"/>
+ </node>
+ </group>
+</launch>
+
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/perception/narrow_stereoproc.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/perception/stereo-perception.launch (from rev 19967, pkg/trunk/sandbox/3dnav_pr2/launch/stereo-perception.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/stereo-perception.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/stereo-perception.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -0,0 +1,29 @@
+<launch>
+
+ <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+ <include file="$(find 3dnav_pr2)/launch/perception/narrow_stereoproc.launch" />
+
+ <!-- remove points corresponding to known objects -->
+ <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <remap from="robot_description" to="robot_description" />
+
+<!-- define a frame that stays fixed for the known objects -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+
+ <remap from="cloud_in" to="/narrow_stereo/cloud" />
+ <remap from="cloud_out" to="full_cloud_without_known_objects" />
+ <param name="object_scale" type="double" value="1.0" />
+ <param name="object_padd" type="double" value="0.025" />
+ </node>
+
+ <!-- add a channel that marks points that are on the robot -->
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robot_description" />
+ <remap from="cloud_in" to="full_cloud_without_known_objects" />
+ <remap from="cloud_out" to="full_cloud_annotated" />
+<!-- <param name="annotate" type="string" value="on_self" /> -->
+ </node>
+
+ <include file="$(find 3dnav_pr2)/launch/perception/collision_map_self_occ.launch" />
+
+</launch>
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/perception/stereo-perception.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/stereo-perception.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/stereo-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/stereo-perception.launch 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/stereo-perception.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -1,29 +0,0 @@
-<launch>
-
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
- <include file="$(find 3dnav_pr2)/launch/narrow_stereoproc.launch" />
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="/narrow_stereo/cloud" />
- <remap from="cloud_out" to="full_cloud_without_known_objects" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.025" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="full_cloud_without_known_objects" />
- <remap from="cloud_out" to="full_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- </node>
-
- <include file="$(find 3dnav_pr2)/launch/collision_map_occ.launch" />
-
-</launch>
Modified: pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
===================================================================
--- pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-07-29 04:31:22 UTC (rev 19975)
@@ -107,13 +107,26 @@
element can have one of the values INSIDE, OUTSIDE or SHADOW. If the value is SHADOW,
the point is on a ray behind the robot and should not have
been seen. If the mask element is INSIDE, the point is inside
- the robot.
+ the robot. The sensor frame is specified to obtain the origin of the sensor.
*/
void maskIntersection(const robot_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask);
+
+ /** \brief Compute the intersection mask for a given pointcloud. If a mask
+ element can have one of the values INSIDE, OUTSIDE or SHADOW. If the value is SHADOW,
+ the point is on a ray behind the robot and should not have
+ been seen. If the mask element is INSIDE, the point is inside
+ the robot. The origin of the sensor is specified as well.
+ */
+ void maskIntersection(const robot_msgs::PointCloud& data_in, const btVector3 &sensor, std::vector<int> &mask);
- /** \brief Assume subsequent calls to getMaskX() will be in the frame passed to this function */
+ /** \brief Assume subsequent calls to getMaskX() will be in the frame passed to this function.
+ * The frame in which the sensor is located is optional */
void assumeFrame(const roslib::Header& header, const std::string &sensor_frame = std::string());
+ /** \brief Assume subsequent calls to getMaskX() will be in the frame passed to this function.
+ * Also specify which possition to assume for the sensor (frame is not needed) */
+ void assumeFrame(const roslib::Header& header, const btVector3 &sensor_pos);
+
/** \brief Get the containment mask (INSIDE or OUTSIDE) value for an individual point. No
setup is performed, assumeFrame() should be called before use */
int getMaskContainment(double x, double y, double z) const;
@@ -156,7 +169,7 @@
void maskAuxContainment(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
/** \brief Perform the actual mask computation. */
- void maskAuxIntersection(const robot_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask);
+ void maskAuxIntersection(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
planning_environment::RobotModels rm_;
tf::TransformListener &tf_;
Modified: pkg/trunk/util/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-07-29 04:31:22 UTC (rev 19975)
@@ -44,6 +44,7 @@
{
// in case configure was called before, we free the memory
freeMemory();
+ sensor_pos_.setValue(0, 0, 0);
// 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
@@ -77,12 +78,12 @@
bspheres_.resize(bodies_.size());
bspheresRadius2_.resize(bodies_.size());
-
+
for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
ROS_DEBUG("Self mask includes link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
ROS_INFO("Self filter using %f padding and %f scaling", padd, scale);
-
+
return true;
}
@@ -108,7 +109,10 @@
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
else
+ {
+ assumeFrame(data_in.header);
maskAuxContainment(data_in, mask);
+ }
}
void robot_self_filter::SelfMask::maskIntersection(const robot_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask)
@@ -117,9 +121,26 @@
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
else
- maskAuxIntersection(data_in, sensor_frame, mask);
+ {
+ assumeFrame(data_in.header, sensor_frame);
+ if (sensor_frame.empty())
+ maskAuxContainment(data_in, mask);
+ else
+ maskAuxIntersection(data_in, mask);
+ }
}
+void robot_self_filter::SelfMask::maskIntersection(const robot_msgs::PointCloud& data_in, const btVector3 &sensor, std::vector<int> &mask)
+{
+ mask.resize(data_in.pts.size());
+ if (bodies_.empty())
+ std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
+ else
+ {
+ assumeFrame(data_in.header, sensor);
+ maskAuxIntersection(data_in, mask);
+ }
+}
void robot_self_filter::SelfMask::computeBoundingSpheres(void)
{
@@ -131,6 +152,12 @@
}
}
+void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const btVector3 &sensor)
+{
+ assumeFrame(header);
+ sensor_pos_ = sensor;
+}
+
void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const std::string &sensor_frame)
{
const unsigned int bs = bodies_.size();
@@ -176,8 +203,6 @@
const unsigned int bs = bodies_.size();
const unsigned int np = data_in.pts.size();
- assumeFrame(data_in.header);
-
// compute a sphere that bounds the entire robot
bodies::BoundingSphere bound;
bodies::mergeBoundingSpheres(bspheres_, bound);
@@ -198,13 +223,11 @@
}
}
-void robot_self_filter::SelfMask::maskAuxIntersection(const robot_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask)
+void robot_self_filter::SelfMask::maskAuxIntersection(const robot_msgs::PointCloud& data_in, std::vector<int> &mask)
{
const unsigned int bs = bodies_.size();
const unsigned int np = data_in.pts.size();
- assumeFrame(data_in.header, sensor_frame);
-
// compute a sphere that bounds the entire robot
bodies::BoundingSphere bound;
bodies::mergeBoundingSpheres(bspheres_, bound);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
