Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21990] pkg/trunk
|
From: <is...@us...> - 2009-08-17 06:42:55
|
Revision: 21990
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21990&view=rev
Author: isucan
Date: 2009-08-17 06:42:49 +0000 (Mon, 17 Aug 2009)
Log Message:
-----------
new feature for robot self filter: removing points at less than a minimum distance to sensor; using this feature when combining pointclouds for the 2 laser sensors (for producing the collision map)
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/robot_self_filter/self_filter.launch
pkg/trunk/util/robot_self_filter/src/self_filter.cpp
pkg/trunk/util/robot_self_filter/src/self_mask.cpp
pkg/trunk/util/robot_self_filter/src/test_filter.cpp
Added Paths:
-----------
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/base_laser_self_filter.launch
Modified: pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-08-17 06:42:49 UTC (rev 21990)
@@ -266,7 +266,7 @@
currentMap_ = obstacles;
// find out which of these points are now occluded
- sf_->assumeFrame(header_, bi_.sensor_frame);
+ sf_->assumeFrame(header_, bi_.sensor_frame, 0.05);
// OpenMP need an int as the lookup variable, but for set,
// this is not possible, so we copy to a vector
Added: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/base_laser_self_filter.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/base_laser_self_filter.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/base_laser_self_filter.launch 2009-08-17 06:42:49 UTC (rev 21990)
@@ -0,0 +1,43 @@
+<launch>
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+
+ <!-- The topic for the input cloud -->
+ <remap from="cloud_in" to="base_scan_cloud_without_known_objects" />
+
+ <!-- The topic for the output cloud -->
+ <remap from="cloud_out" to="base_scan_cloud_filtered" />
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="base_laser" />
+
+ <!-- Minimum distance to sensor for point not to be considered inside -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
+ <!-- The padding to be added for the body parts the robot can see -->
+ <param name="self_see_padd" type="double" value="0.02" />
+
+ <!-- The scaling to be added for the body parts the robot can see -->
+ <param name="self_see_scale" type="double" value="1.0" />
+
+ <!-- The names of the links the sensor can see -->
+ <param name="self_see_links" type="string" value="l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link" />
+
+ </node>
+</launch>
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch 2009-08-17 06:42:49 UTC (rev 21990)
@@ -11,6 +11,9 @@
filtered; This parameter is optional. If it is not specified,
shadow points will be considered outside -->
<param name="sensor_frame" type="string" value="laser_tilt_link" />
+
+ <!-- Minimum distance to sensor for point not to be considered inside -->
+ <param name="min_sensor_dist" type="double" value="0.05" />
<!-- The padding to be added for the body parts the robot can see -->
<param name="self_see_padd" type="double" value="0.07" />
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-17 06:42:49 UTC (rev 21990)
@@ -9,16 +9,15 @@
<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 machine="three" pkg="laser_scan" type="scan_to_cloud" output="screen">
+ <!-- convert tilt laser scan to pointcloud -->
+ <node machine="three" pkg="laser_scan" type="scan_to_cloud" output="screen" name="scan_to_cloud_tilt_laser">
<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 machine="three" pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <node machine="three" pkg="planning_environment" type="clear_known_objects" respawn="true" output="screen">
<remap from="robot_description" to="robot_description" />
<!-- define a frame that stays fixed for the known objects -->
@@ -51,10 +50,52 @@
<node machine="four" 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_tilt_cloud_filtered" />
</node>
+ <!-- convert base laser scan to pointcloud -->
+ <node machine="three" pkg="laser_scan" type="scan_to_cloud" respawn="true" name="scan_to_cloud_base_laser">
+ <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
+ <param name="scan_topic" value="base_scan" />
+ <param name="cloud_topic" value="base_scan_cloud" />
+ </node>
+
+ <!-- remove points corresponding to known objects -->
+ <node machine="three" pkg="planning_environment" type="clear_known_objects" respawn="true" output="screen">
+
+<!-- define a frame that stays fixed for the known objects -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="base_link" />
+
+ <remap from="cloud_in" to="base_scan_cloud" />
+ <remap from="cloud_out" to="base_scan_cloud_without_known_objects" />
+ <param name="object_scale" type="double" value="1.0" />
+ <param name="object_padd" type="double" value="0.05" />
+ </node>
+
+ <include file="$(find 3dnav_pr2)/launch/perception/bits/base_laser_self_filter.launch" />
+
+ <node machine="four" pkg="point_cloud_assembler" type="merge_clouds" output="screen">
+
+ <!-- first input cloud -->
+ <remap from="cloud_in1" to="full_tilt_cloud_filtered"/>
+
+ <!-- second input cloud -->
+ <remap from="cloud_in2" to="base_scan_cloud_filtered"/>
+
+ <!-- output cloud -->
+ <remap from="cloud_out" to="full_cloud_filtered"/>
+
+ <!-- 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/bits/collision_map_self_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-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-08-17 06:42:49 UTC (rev 21990)
@@ -108,8 +108,8 @@
the origin of the sensor. A callback can be registered for
the first intersection point on each body.
*/
- void maskIntersection(const sensor_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask,
- const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
+ void maskIntersection(const sensor_msgs::PointCloud& data_in, const std::string &sensor_frame, const double min_sensor_dist,
+ std::vector<int> &mask, const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
/** \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,
@@ -117,16 +117,20 @@
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 sensor_msgs::PointCloud& data_in, const btVector3 &sensor, std::vector<int> &mask,
- const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
+ void maskIntersection(const sensor_msgs::PointCloud& data_in, const btVector3 &sensor, const double min_sensor_dist,
+ std::vector<int> &mask, const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
/** \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());
+ void assumeFrame(const roslib::Header& header);
/** \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, const double min_sensor_dist);
+
+ /** \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);
+ void assumeFrame(const roslib::Header& header, const btVector3 &sensor_pos, const double min_sensor_dist);
/** \brief Get the containment mask (INSIDE or OUTSIDE) value for an individual point. No
setup is performed, assumeFrame() should be called before use */
@@ -171,6 +175,7 @@
ros::NodeHandle nh_;
btVector3 sensor_pos_;
+ double min_sensor_dist_;
std::vector<SeeLink> bodies_;
std::vector<double> bspheresRadius2_;
Modified: pkg/trunk/util/robot_self_filter/self_filter.launch
===================================================================
--- pkg/trunk/util/robot_self_filter/self_filter.launch 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/self_filter.launch 2009-08-17 06:42:49 UTC (rev 21990)
@@ -14,7 +14,10 @@
filtered; This parameter is optional. If it is not specified,
shadow points will be considered outside -->
<param name="sensor_frame" type="string" value="laser_tilt_link" />
-
+
+ <!-- Minimum distance to sensor (for point not to be considered inside) -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
<!-- The padding to be added for the body parts the robot can see -->
<param name="self_see_padd" type="double" value="0.02" />
Modified: pkg/trunk/util/robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-08-17 06:42:49 UTC (rev 21990)
@@ -46,6 +46,7 @@
SelfFilter(void)
{
nh_.param<std::string>("~sensor_frame", sensor_frame_, std::string());
+ nh_.param<double>("~min_sensor_dist", min_sensor_dist_, 0.01);
std::vector<std::string> links;
std::string link_names;
@@ -77,7 +78,7 @@
if (!annotate_.empty())
ROS_INFO("Self filter is adding annotation channel '%s'", annotate_.c_str());
if (!sensor_frame_.empty())
- ROS_INFO("Self filter is removing shadow points for sensor in frame '%s'", sensor_frame_.c_str());
+ ROS_INFO("Self filter is removing shadow points for sensor in frame '%s'. Minimum distance to sensor is %f.", sensor_frame_.c_str(), min_sensor_dist_);
}
~SelfFilter(void)
@@ -101,7 +102,7 @@
if (sensor_frame_.empty())
sf_->maskContainment(*cloud, mask);
else
- sf_->maskIntersection(*cloud, sensor_frame_, mask);
+ sf_->maskIntersection(*cloud, sensor_frame_, min_sensor_dist_, mask);
double sec = (ros::WallTime::now() - tm).toSec();
@@ -183,6 +184,8 @@
std::string sensor_frame_;
ros::NodeHandle nh_;
std::string annotate_;
+ double min_sensor_dist_;
+
};
Modified: pkg/trunk/util/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-08-17 06:42:49 UTC (rev 21990)
@@ -115,15 +115,15 @@
}
}
-void robot_self_filter::SelfMask::maskIntersection(const sensor_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask,
- const boost::function<void(const btVector3&)> &callback)
+void robot_self_filter::SelfMask::maskIntersection(const sensor_msgs::PointCloud& data_in, const std::string &sensor_frame, const double min_sensor_dist,
+ std::vector<int> &mask, const boost::function<void(const btVector3&)> &callback)
{
mask.resize(data_in.points.size());
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
else
{
- assumeFrame(data_in.header, sensor_frame);
+ assumeFrame(data_in.header, sensor_frame, min_sensor_dist);
if (sensor_frame.empty())
maskAuxContainment(data_in, mask);
else
@@ -131,15 +131,15 @@
}
}
-void robot_self_filter::SelfMask::maskIntersection(const sensor_msgs::PointCloud& data_in, const btVector3 &sensor, std::vector<int> &mask,
- const boost::function<void(const btVector3&)> &callback)
+void robot_self_filter::SelfMask::maskIntersection(const sensor_msgs::PointCloud& data_in, const btVector3 &sensor_pos, const double min_sensor_dist,
+ std::vector<int> &mask, const boost::function<void(const btVector3&)> &callback)
{
mask.resize(data_in.points.size());
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
else
{
- assumeFrame(data_in.header, sensor);
+ assumeFrame(data_in.header, sensor_pos, min_sensor_dist);
maskAuxIntersection(data_in, mask, callback);
}
}
@@ -154,14 +154,35 @@
}
}
-void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const btVector3 &sensor)
+void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const btVector3 &sensor_pos, double min_sensor_dist)
{
assumeFrame(header);
- sensor_pos_ = sensor;
+ sensor_pos_ = sensor_pos;
+ min_sensor_dist_ = min_sensor_dist;
}
-void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const std::string &sensor_frame)
+void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const std::string &sensor_frame, double min_sensor_dist)
{
+ assumeFrame(header);
+
+ // compute the origin of the sensor in the frame of the cloud
+ try
+ {
+ tf::Stamped<btTransform> transf;
+ tf_.lookupTransform(header.frame_id, sensor_frame, 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(), header.frame_id.c_str());
+ }
+
+ min_sensor_dist_ = min_sensor_dist;
+}
+
+void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header)
+{
const unsigned int bs = bodies_.size();
// place the links in the assumed frame
@@ -185,22 +206,6 @@
}
computeBoundingSpheres();
-
- // compute the origin of the sensor in the frame of the cloud
- if (!sensor_frame.empty())
- {
- try
- {
- tf::Stamped<btTransform> transf;
- tf_.lookupTransform(header.frame_id, sensor_frame, 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(), header.frame_id.c_str());
- }
- }
}
void robot_self_filter::SelfMask::maskAuxContainment(const sensor_msgs::PointCloud& data_in, std::vector<int> &mask)
@@ -257,29 +262,35 @@
{
// we check it the point is a shadow point
btVector3 dir(sensor_pos_ - pt);
- dir.normalize();
- if (callback)
- {
- std::vector<btVector3> intersections;
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
- {
- callback(intersections[0]);
- out = SHADOW;
- }
- }
+ btScalar lng = dir.length();
+ if (lng < min_sensor_dist_)
+ out = INSIDE;
else
- {
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->intersectsRay(pt, dir))
- out = SHADOW;
+ {
+ dir /= lng;
+ if (callback)
+ {
+ std::vector<btVector3> intersections;
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
+ {
+ callback(intersections[0]);
+ out = SHADOW;
+ }
+ }
+ else
+ {
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->intersectsRay(pt, dir))
+ out = SHADOW;
+ }
+
+ // if it is not a shadow point, we check if it is inside the scaled body
+ if (out == OUTSIDE && bound.center.distance2(pt) < radiusSquared)
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->containsPoint(pt))
+ out = INSIDE;
}
-
- // if it is not a shadow point, we check if it is inside the scaled body
- if (out == OUTSIDE && bound.center.distance2(pt) < radiusSquared)
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->containsPoint(pt))
- out = INSIDE;
}
mask[i] = out;
@@ -317,28 +328,35 @@
// we check it the point is a shadow point
btVector3 dir(sensor_pos_ - pt);
- dir.normalize();
- if (callback)
- {
- std::vector<btVector3> intersections;
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
- {
- callback(intersections[0]);
- out = SHADOW;
- }
- }
+ btScalar lng = dir.length();
+ if (lng < min_sensor_dist_)
+ out = INSIDE;
else
{
+ dir /= lng;
+
+ if (callback)
+ {
+ std::vector<btVector3> intersections;
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
+ {
+ callback(intersections[0]);
+ out = SHADOW;
+ }
+ }
+ else
+ {
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->intersectsRay(pt, dir))
+ out = SHADOW;
+ }
+
+ // if it is not a shadow point, we check if it is inside the scaled body
for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->intersectsRay(pt, dir))
- out = SHADOW;
+ if (bodies_[j].body->containsPoint(pt))
+ out = INSIDE;
}
-
- // if it is not a shadow point, we check if it is inside the scaled body
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->containsPoint(pt))
- out = INSIDE;
}
return out;
}
Modified: pkg/trunk/util/robot_self_filter/src/test_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-08-17 06:42:49 UTC (rev 21990)
@@ -118,7 +118,7 @@
ros::WallTime tm = ros::WallTime::now();
std::vector<int> mask;
- sf_->maskIntersection(in, "laser_tilt_mount_link", mask, boost::bind(&TestSelfFilter::gotIntersection, this, _1) );
+ sf_->maskIntersection(in, "laser_tilt_mount_link", 0.01, mask, boost::bind(&TestSelfFilter::gotIntersection, this, _1) );
// sf_->maskContainment(in, mask);
printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
