Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17683] pkg/trunk
|
From: <is...@us...> - 2009-06-25 18:28:25
|
Revision: 17683
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17683&view=rev
Author: isucan
Date: 2009-06-25 18:28:22 +0000 (Thu, 25 Jun 2009)
Log Message:
-----------
using annotated pointclouds
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
pkg/trunk/mapping/collision_map/collision_map_occ.launch
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/util/robot_self_filter/src/self_filter.cpp
Modified: pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch 2009-06-25 18:11:51 UTC (rev 17682)
+++ pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch 2009-06-25 18:28:22 UTC (rev 17683)
@@ -13,11 +13,12 @@
<node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
<remap from="robot_description" to="robotdesc/pr2" />
<remap from="cloud_in" to="tilt_scan_cloud" />
- <remap from="cloud_out" to="tilt_scan_cloud_filtered" />
+ <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
+ <param name="annotate" type="string" value="on_self" />
</node>
<node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
- <remap from="scan_in" to="tilt_scan_cloud_filtered"/>
+ <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" />
@@ -29,9 +30,9 @@
<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_filtered" />
+ <remap from="full_cloud" to="full_cloud_annotated" />
</node>
- <include file="$(find collision_map)/collision_map.launch" /> <!-- USE collision_map not collision_map_occ -->
+ <include file="$(find collision_map)/collision_map_occ.launch" />
</launch>
Modified: pkg/trunk/mapping/collision_map/collision_map_occ.launch
===================================================================
--- pkg/trunk/mapping/collision_map/collision_map_occ.launch 2009-06-25 18:11:51 UTC (rev 17682)
+++ pkg/trunk/mapping/collision_map/collision_map_occ.launch 2009-06-25 18:28:22 UTC (rev 17683)
@@ -1,8 +1,11 @@
<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_filtered" />
+ <remap from="cloud_in" to="full_cloud_annotated" />
<remap from="robot_description" to="robotdesc/pr2" />
+ <!-- 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="base_link" />
Modified: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-06-25 18:11:51 UTC (rev 17682)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-06-25 18:28:22 UTC (rev 17683)
@@ -54,6 +54,62 @@
CollisionMapperOcc(void) : sf_(tf_),
mn_(tf_, boost::bind(&CollisionMapperOcc::cloudCallback, this, _1), "cloud_in", "", 1)
{
+ loadParams();
+
+ // configure the self mask and the message notifier
+ std::vector<std::string> frames;
+ sf_.getLinkFrames(frames);
+ if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end())
+ frames.push_back(robotFrame_);
+ mn_.setTargetFrame(frames);
+
+ // advertise our topic
+ cmapPublisher_ = nh_.advertise<robot_msgs::CollisionMap>("collision_map_occ", 1);
+ }
+
+private:
+
+ struct CollisionPoint
+ {
+ int x, y, z;
+ };
+
+ struct CollisionPointOrder
+ {
+ bool operator()(const CollisionPoint &a, const CollisionPoint &b)
+ {
+ 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;
+ }
+ };
+
+ // parameters & precomputed values for the box that represents the collision map
+ // around the robot
+ struct BoxInfo
+ {
+ double dimensionX, dimensionY, dimensionZ;
+ double originX, originY, originZ;
+ double sensorX, sensorY, sensorZ;
+ double resolution;
+ int radius;
+ int sx, sy, sz;
+ int minX, minY, minZ;
+ int maxX, maxY, maxZ;
+ double real_minX, real_minY, real_minZ;
+ double real_maxX, real_maxY, real_maxZ;
+ };
+
+ typedef std::set<CollisionPoint, CollisionPointOrder> CMap;
+
+ void loadParams(void)
+ {
// a frame that does not move with the robot
nh_.param<std::string>("~fixed_frame", fixedFrame_, "odom");
@@ -85,7 +141,9 @@
"sensor is at (%f, %f, %f), fixed fame is '%s', radius for raytraced occlusions is %d.",
robotFrame_.c_str(), bi_.dimensionX, bi_.dimensionY, bi_.dimensionZ, bi_.originX, bi_.originY, bi_.originZ, bi_.resolution,
bi_.sensorX, bi_.sensorY, bi_.sensorZ, fixedFrame_.c_str(), bi_.radius);
-
+
+ nh_.param<std::string>("~cloud_annotation", cloud_annotation_, std::string());
+
// compute some useful values
bi_.sx = (int)(0.5 + (bi_.sensorX - bi_.originX) / bi_.resolution);
bi_.sy = (int)(0.5 + (bi_.sensorY - bi_.originY) / bi_.resolution);
@@ -105,61 +163,37 @@
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;
-
-
- // configure the self mask and the message notifier
- std::vector<std::string> frames;
- sf_.getLinkFrames(frames);
- if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end())
- frames.push_back(robotFrame_);
- mn_.setTargetFrame(frames);
-
- // advertise our topic
- cmapPublisher_ = nh_.advertise<robot_msgs::CollisionMap>("collision_map_occ", 1);
+ bi_.real_maxZ = bi_.dimensionZ + bi_.originZ;
}
-private:
-
- struct CollisionPoint
+ void computeCloudMask(const robot_msgs::PointCloud &cloud, std::vector<bool> &mask)
{
- int x, y, z;
- };
-
- struct CollisionPointOrder
- {
- bool operator()(const CollisionPoint &a, const CollisionPoint &b)
+ if (cloud_annotation_.empty())
+ sf_.mask(cloud, mask);
+ else
{
- 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;
+ int c = -1;
+ for (unsigned int i = 0 ; i < cloud.chan.size() ; ++i)
+ if (cloud.chan[i].name == cloud_annotation_)
+ {
+ c = i;
+ break;
+ }
+ if (c < 0)
+ {
+ ROS_WARN("Cloud annotation channel '%s' is missing", cloud_annotation_.c_str());
+ sf_.mask(cloud, mask);
+ }
+ else
+ {
+ ROS_ASSERT(cloud.chan[c].vals.size() == cloud.pts.size());
+ mask.resize(cloud.pts.size());
+ for (unsigned int i = 0 ; i < mask.size() ; ++i)
+ mask[i] = cloud.chan[c].vals[i] > 0.0;
+ }
}
- };
-
- // parameters & precomputed values for the box that represents the collision map
- // around the robot
- struct BoxInfo
- {
- double dimensionX, dimensionY, dimensionZ;
- double originX, originY, originZ;
- double sensorX, sensorY, sensorZ;
- double resolution;
- int radius;
- int sx, sy, sz;
- int minX, minY, minZ;
- int maxX, maxY, maxZ;
- double real_minX, real_minY, real_minZ;
- double real_maxX, real_maxY, real_maxZ;
- };
+ }
- typedef std::set<CollisionPoint, CollisionPointOrder> CMap;
-
void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
{
std::vector<bool> mask;
@@ -181,7 +215,7 @@
{
// separate the received points into ones on the robot and ones that are obstacles
// the frame of the cloud does not matter here
- sf_.mask(*cloud, mask);
+ computeCloudMask(*cloud, mask);
}
}
@@ -616,7 +650,8 @@
ros::NodeHandle nh_;
ros::Publisher cmapPublisher_;
roslib::Header header_;
-
+ std::string cloud_annotation_;
+
CMap currentMap_;
BoxInfo bi_;
std::string fixedFrame_;
Modified: pkg/trunk/util/robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-06-25 18:11:51 UTC (rev 17682)
+++ pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-06-25 18:28:22 UTC (rev 17683)
@@ -47,7 +47,7 @@
std::vector<std::string> frames;
sf_.getLinkFrames(frames);
mn_.setTargetFrame(frames);
- nh_.param<std::string>("/self_filter/annotate", annotate_, std::string());
+ nh_.param<std::string>("~annotate", annotate_, std::string());
pointCloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("cloud_out", 1);
if (!annotate_.empty())
ROS_INFO("Self filter is adding annotation channel '%s'", annotate_.c_str());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
