|
From: <tpr...@us...> - 2008-12-06 03:28:48
|
Revision: 7721
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7721&view=rev
Author: tpratkanis
Date: 2008-12-06 03:28:44 +0000 (Sat, 06 Dec 2008)
Log Message:
-----------
Make world_3d_map create point clouds: a change needed for fixing arm demos.
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-12-06 02:51:27 UTC (rev 7720)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-12-06 03:28:44 UTC (rev 7721)
@@ -88,7 +88,7 @@
public:
- NodeRobotModel(ros::node *node, const std::string &robot_model) : m_tf(*node, true, 1 * 1000000000ULL, 0ULL)
+ NodeRobotModel(ros::node *node, const std::string &robot_model) : m_tf(*node, true, 1 * 1000000000ULL, 1000000000ULL)
{
m_urdf = NULL;
m_kmodel = NULL;
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-12-06 02:51:27 UTC (rev 7720)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-12-06 03:28:44 UTC (rev 7721)
@@ -147,7 +147,7 @@
/* Set up the transform client */
double laser_x_offset;
param("laser_x_offset", laser_x_offset, 0.275);
- m_tf.setWithEulers("base_laser", "base", laser_x_offset, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
+ m_tf.setWithEulers("base_laser", "base_link", laser_x_offset, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
/* create a thread that handles the publishing of the data */
m_publishingThread = ros::thread::member_thread::startMemberFunctionThread<World3DMap>(this, &World3DMap::publishDataThread);
@@ -193,7 +193,7 @@
{
if(!m_robotState)
{
- //ROS_WARN("Ignoring state update because I haven't yet received the robot description");
+ ROS_WARN("Ignoring state update because I haven't yet received the robot description");
return;
}
//m_robotState->print();
@@ -224,17 +224,64 @@
m_projector.projectLaser(m_baseScanMsg, local_cloud, m_baseLaserMaxRange);
processData(local_cloud);
}
-
+
+
void publishDataThread(void)
{
ros::Duration *d = new ros::Duration();
d->fromSec(1.0/m_maxPublishFrequency);
+
+ /* while everything else is running (map building) check if
+ there are any updates to send, but do so at most at the
+ maximally allowed frequency of sending data */
+ while (m_active)
+ {
+ d->sleep();
- /* Pump out buffered, filtered point clouds and clear the buffer */
+ m_worldDataMutex.lock();
+ if (m_active && m_currentWorld.size() > 0)
+ {
+ std_msgs::PointCloud toPublish;
+ toPublish.header = m_currentWorld.back()->header;
+
+ unsigned int npts = 0;
+ for (unsigned int i = 0 ; i < m_currentWorld.size() ; ++i)
+ npts += m_currentWorld[i]->get_pts_size();
+
+ toPublish.set_pts_size(npts);
+
+ unsigned int j = 0;
+ for (unsigned int i = 0 ; i < m_currentWorld.size() ; ++i)
+ {
+ unsigned int n = m_currentWorld[i]->get_pts_size();
+ for (unsigned int k = 0 ; k < n ; ++k)
+ toPublish.pts[j++] = m_currentWorld[i]->pts[k];
+ }
+
+ toPublish.set_pts_size(j);
+ if (ok())
+ {
+ if (m_verbose)
+ ROS_INFO("Publishing a point cloud with %u points\n", toPublish.get_pts_size());
+ publish("world_3d_map", toPublish);
+ }
+ }
+ m_worldDataMutex.unlock();
+ }
+ delete d;
+ }
+
+ /*void publishDataThread(void)
+ {
+ ros::Duration *d = new ros::Duration(1.0/m_maxPublishFrequency);
+
+ /* Pump out buffered, filtered point clouds and clear the buffer
while (m_active)
{
+ ROS_INFO("Loop\n");
for(unsigned int i = 0; i < m_currentWorld.size(); i++){
std_msgs::PointCloud* p = m_currentWorld[i];
+ ROS_INFO("Publish\n");
publish("world_3d_map", *p);
delete p;
}
@@ -245,7 +292,7 @@
}
delete d;
- }
+ }*/
void addSelfSeeBodies(void)
{
@@ -309,6 +356,7 @@
void processData(const std_msgs::PointCloud& local_cloud)
{
if (!m_acceptScans){
+ ROS_INFO("Rejecting scans\n");
return;
}
@@ -320,11 +368,6 @@
const std_msgs::PointCloud& point_cloud = point_clouds_.front();
- //make sure that we don't fall to far in the past
- if(ros::Time::now() - point_cloud.header.stamp > ros::Duration(9, 0)){
- point_clouds_.pop_front();
- continue;
- }
std_msgs::PointCloud map_cloud;
@@ -340,7 +383,7 @@
}
catch(libTF::TransformReference::ExtrapolateException& ex)
{
- //ROS_ERROR("Extrapolation exception: %s\n", ex.what());
+ ROS_ERROR("Extrapolation exception: %s\n", ex.what());
break;
}
catch(libTF::TransformReference::ConnectivityException& ex)
@@ -406,7 +449,7 @@
copy->pts[j++] = cloud.pts[k];
copy->set_pts_size(j);
- ROS_DEBUG("Filter 0 discarded %d points (%d left) \n", n - j, j);
+ ROS_INFO("Filter 0 discarded %d points (%d left) \n", n - j, j);
return copy;
}
@@ -443,7 +486,7 @@
}
}
- ROS_DEBUG("Filter 1 discarded %d points (%d left) \n", n - j, j);
+ ROS_INFO("Filter 1 discarded %d points (%d left) \n", n - j, j);
copy->set_pts_size(j);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|