|
From: <is...@us...> - 2009-06-19 19:22:52
|
Revision: 17367
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17367&view=rev
Author: isucan
Date: 2009-06-19 19:22:50 +0000 (Fri, 19 Jun 2009)
Log Message:
-----------
broadcasting single line pointclouds
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/self_filter.launch
pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp
Modified: pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch 2009-06-19 19:22:50 UTC (rev 17367)
@@ -1,6 +1,7 @@
<launch>
<include file="$(find pr2_defs)/pr2_planning_environment.launch" />
+ <include file="$(find pr2_laser_tilt)/laser_tilt_controller.launch" />
<node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
<remap from="scan_in" to="tilt_scan"/>
@@ -18,10 +19,26 @@
<remap from="full_cloud" to="snapshot_cloud" />
</node>
- <include file="$(find pr2_laser_tilt)/laser_tilt_controller.launch" />
+ <node pkg="new_robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robotdesc/pr2" />
+ <remap from="cloud_in" to="snapshot_cloud" />
+ <remap from="cloud_out" to="snapshot_cloud_filtered" />
+ </node>
- <node pkg="new_robot_self_filter" type="self_filter" args="robot_description:=robotdesc/pr2 full_cloud:=snapshot_cloud" respawn="true" output="screen" />
+ <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>
+
+ <node pkg="new_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" />
+ </node>
+
<include file="$(find collision_map)/collision_map.launch" />
-
+
</launch>
Modified: pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-06-19 19:22:50 UTC (rev 17367)
@@ -68,7 +68,7 @@
ROS_INFO("Inverting filter output");
bool accurate;
- nh_.param("~accurate_timing", accurate, true);
+ nh_.param("~accurate_timing", accurate, false);
if (accurate)
ROS_INFO("Using accurate timing");
Modified: pkg/trunk/util/new_robot_self_filter/self_filter.launch
===================================================================
--- pkg/trunk/util/new_robot_self_filter/self_filter.launch 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/util/new_robot_self_filter/self_filter.launch 2009-06-19 19:22:50 UTC (rev 17367)
@@ -1,4 +1,8 @@
<launch>
<include file="$(find pr2_defs)/pr2_planning_environment.launch" />
- <node pkg="new_robot_self_filter" type="self_filter" args="robot_description:=robotdesc/pr2 full_cloud:=snapshot_cloud" respawn="true" output="screen" />
+ <node pkg="new_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" />
+ </node>
</launch>
Modified: pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 19:22:50 UTC (rev 17367)
@@ -41,13 +41,15 @@
{
public:
- SelfFilter(void) : tf_(ros::Duration(10.0))
+ SelfFilter(void)
{
sf_.configure();
- pointCloudSubscriber_ = nh_.subscribe("full_cloud", 1, &SelfFilter::cloudCallback, this);
- pointCloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("full_cloud_filtered", 1);
+ pointCloudSubscriber_ = nh_.subscribe("cloud_in", 1, &SelfFilter::cloudCallback, this);
+ pointCloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("cloud_out", 1);
}
+private:
+
void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
{
robot_msgs::PointCloud out;
@@ -58,9 +60,6 @@
pointCloudPublisher_.publish(out);
}
-private:
-
- tf::TransformListener tf_;
filters::SelfFilter<robot_msgs::PointCloud> sf_;
ros::Publisher pointCloudPublisher_;
ros::Subscriber pointCloudSubscriber_;
@@ -70,13 +69,10 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv, "self_filter");
+ ros::init(argc, argv, "self_filter", ros::init_options::AnonymousName);
SelfFilter s;
ros::spin();
return 0;
}
-
-
-
Modified: pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp 2009-06-19 19:22:50 UTC (rev 17367)
@@ -139,7 +139,7 @@
float maxT = *std::max_element(times.vals.begin(), times.vals.end());
if (maxT <= FLT_MIN)
{
- ROS_WARN("'stamp' channel contains invalid data");
+ ROS_WARN("'stamps' channel contains invalid data");
maskSimple(data_in, mask);
return;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|