Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18865] pkg/trunk
|
From: <is...@us...> - 2009-07-15 18:22:40
|
Revision: 18865
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18865&view=rev
Author: isucan
Date: 2009-07-15 18:22:37 +0000 (Wed, 15 Jul 2009)
Log Message:
-----------
removing points on known object models seems to work
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
pkg/trunk/mapping/collision_map/collision_map_occ.launch
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-15 18:22:37 UTC (rev 18865)
@@ -1,24 +1,40 @@
<launch>
+ <!-- send additional description parameters -->
<include file="$(find pr2_defs)/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="control_laser.py" args="laser_tilt_controller sine 15 .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="robotdesc/pr2" />
+
+<!-- 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" />
+ </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="robotdesc/pr2" />
- <remap from="cloud_in" to="tilt_scan_cloud" />
+ <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" />
</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" />
@@ -35,6 +51,7 @@
<remap from="full_cloud" to="full_cloud_annotated" />
</node>
+ <!-- start collision map -->
<include file="$(find collision_map)/collision_map_occ.launch" />
</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch 2009-07-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch 2009-07-15 18:22:37 UTC (rev 18865)
@@ -3,9 +3,21 @@
<include file="$(find pr2_defs)/pr2_planning_environment.launch" />
<include file="$(find stereo_image_proc)/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="robotdesc/pr2" />
+
+<!-- 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" />
+ </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="robotdesc/pr2" />
- <remap from="cloud_in" to="/narrow_stereo/cloud" />
+ <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>
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-15 18:22:37 UTC (rev 18865)
@@ -90,14 +90,20 @@
o1.header.frame_id = "/base_link";
o1.header.stamp = ros::Time::now();
o1.id = "Part1";
- o1.object.type = mapping_msgs::Object::SPHERE;
- o1.object.dimensions.resize(1);
- o1.object.dimensions[0] = 0.32;
- // o1.object.dimensions[1] = 0.32;
- // o1.object.dimensions[2] = 0.32;
- o1.pose.position.x = 0.7;
- o1.pose.position.z = 0.6;
+ o1.object.type = mapping_msgs::Object::BOX;
+ o1.object.dimensions.resize(3);
+ o1.object.dimensions[0] = 0.3;
+ o1.object.dimensions[1] = 0.3;
+ o1.object.dimensions[2] = 0.3;
+ o1.pose.position.x = 1.4;
+ o1.pose.position.y = 0.0;
+ o1.pose.position.z = 0.0;
+ o1.pose.orientation.x = 0.0;
+ o1.pose.orientation.y = 0.0;
+ o1.pose.orientation.z = 0.0;
+ o1.pose.orientation.w = 1.0;
+
while(1){
pub.publish(o1); sleep(1);
}
Modified: pkg/trunk/mapping/collision_map/collision_map_occ.launch
===================================================================
--- pkg/trunk/mapping/collision_map/collision_map_occ.launch 2009-07-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/mapping/collision_map/collision_map_occ.launch 2009-07-15 18:22:37 UTC (rev 18865)
@@ -11,6 +11,7 @@
<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 -->
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-07-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-07-15 18:22:37 UTC (rev 18865)
@@ -66,6 +66,8 @@
nh_.param<double>("~object_scale", scale_, 1.0);
nh_.param<double>("~object_padd", padd_, 0.02);
+ ROS_INFO("Clearing points on known objects using '%s' as fixed frame, %f padding and %f scaling", fixed_frame_.c_str(), padd_, scale_);
+
cloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("cloud_out", 1);
kmsm_->setOnAfterAttachBodyCallback(boost::bind(&ClearKnownObjects::attachObjectEvent, this, _1));
cloudNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(tf_, boost::bind(&ClearKnownObjects::cloudCallback, this, _1), "cloud_in", fixed_frame_, 1);
@@ -125,10 +127,8 @@
double rsquare;
};
- void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ void computeMask(const robot_msgs::PointCloud &cloud, std::vector<int> &mask)
{
- updateObjects_.lock();
-
// check if we have attached bodies
if (attachedObjects_.size() > 0)
{
@@ -174,18 +174,23 @@
kmsm_->getKinematicModel()->unlock();
}
- // transform pointcloud into fixed frame
- robot_msgs::PointCloud cloudTransf;
- tf_.transformPointCloud(fixed_frame_, *cloud, cloudTransf);
+ // transform pointcloud into fixed frame, if needed
+ robot_msgs::PointCloud temp;
+ const robot_msgs::PointCloud *cloudTransf = &cloud;
+ if (fixed_frame_ != cloud.header.frame_id)
+ {
+ tf_.transformPointCloud(fixed_frame_, cloud, temp);
+ cloudTransf = &temp;
+ }
// compute mask for cloud
- int n = cloud->pts.size();
- std::vector<int> mask(n);
+ int n = cloud.pts.size();
+ mask.resize(n);
#pragma omp parallel for
for (int i = 0 ; i < n ; ++i)
{
- btVector3 pt = btVector3(cloudTransf.pts[i].x, cloudTransf.pts[i].y, cloudTransf.pts[i].z);
+ btVector3 pt = btVector3(cloudTransf->pts[i].x, cloudTransf->pts[i].y, cloudTransf->pts[i].z);
int out = 1;
for (unsigned int j = 0 ; out && j < attachedObjects_.size() ; ++j)
@@ -200,38 +205,61 @@
mask[i] = out;
}
+ }
+
+ void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ {
+ std::vector<int> mask;
+ bool filter = false;
- updateObjects_.unlock();
+ updateObjects_.lock();
- // publish new cloud
- const unsigned int np = cloud->pts.size();
- robot_msgs::PointCloud data_out;
-
- // fill in output data with points that are NOT in the known objects
- data_out.header = cloud->header;
-
- data_out.pts.resize(0);
- data_out.pts.reserve(np);
-
- data_out.chan.resize(cloud->chan.size());
- for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
+ if (attachedObjects_.size() > 0 || objectsInMap_.size() > 0)
{
- ROS_ASSERT(cloud->chan[i].vals.size() == cloud->pts.size());
- data_out.chan[i].name = cloud->chan[i].name;
- data_out.chan[i].vals.reserve(cloud->chan[i].vals.size());
+ computeMask(*cloud, mask);
+ filter = true;
}
- for (unsigned int i = 0 ; i < np ; ++i)
- if (mask[i])
+ updateObjects_.unlock();
+
+ if (filter)
+ {
+ // publish new cloud
+ const unsigned int np = cloud->pts.size();
+ robot_msgs::PointCloud data_out;
+
+ // fill in output data with points that are NOT in the known objects
+ data_out.header = cloud->header;
+
+ data_out.pts.resize(0);
+ data_out.pts.reserve(np);
+
+ data_out.chan.resize(cloud->chan.size());
+ for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
{
- data_out.pts.push_back(cloud->pts[i]);
- for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
- data_out.chan[j].vals.push_back(cloud->chan[j].vals[i]);
+ ROS_ASSERT(cloud->chan[i].vals.size() == cloud->pts.size());
+ data_out.chan[i].name = cloud->chan[i].name;
+ data_out.chan[i].vals.reserve(cloud->chan[i].vals.size());
}
-
- cloudPublisher_.publish(data_out);
+
+ for (unsigned int i = 0 ; i < np ; ++i)
+ if (mask[i])
+ {
+ data_out.pts.push_back(cloud->pts[i]);
+ for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
+ data_out.chan[j].vals.push_back(cloud->chan[j].vals[i]);
+ }
+
+ ROS_DEBUG("Published filtered cloud (%d points out of %d)", data_out.pts.size(), cloud->pts.size());
+ cloudPublisher_.publish(data_out);
+ }
+ else
+ {
+ cloudPublisher_.publish(*cloud);
+ ROS_DEBUG("Republished unchanged cloud");
+ }
}
-
+
void objectInMapCallback(const mapping_msgs::ObjectInMapConstPtr &objectInMap)
{
updateObjects_.lock();
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp 2009-07-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp 2009-07-15 18:22:37 UTC (rev 18865)
@@ -138,7 +138,7 @@
{
case mapping_msgs::Object::SPHERE:
mk.type = visualization_msgs::Marker::SPHERE;
- mk.scale.x = mk.scale.y = mk.scale.z = obj.dimensions[0];
+ mk.scale.x = mk.scale.y = mk.scale.z = obj.dimensions[0] * 2.0;
break;
case mapping_msgs::Object::BOX:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
