Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3034] pkg/trunk
|
From: <is...@us...> - 2008-08-14 00:46:39
|
Revision: 3034
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3034&view=rev
Author: isucan
Date: 2008-08-14 00:46:47 +0000 (Thu, 14 Aug 2008)
Log Message:
-----------
fixing little glitches...
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-14 00:29:03 UTC (rev 3033)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-14 00:46:47 UTC (rev 3034)
@@ -92,7 +92,10 @@
virtual ~NodeWithODECollisionModel(void)
{
if (m_collisionSpace)
+ {
delete m_collisionSpace;
+ m_kmodel = NULL;
+ }
}
virtual void setRobotDescription(robot_desc::URDF *file)
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-08-14 00:29:03 UTC (rev 3033)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-08-14 00:46:47 UTC (rev 3034)
@@ -88,7 +88,7 @@
m_urdf = NULL;
m_kmodel = NULL;
m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
- loadRobotDescription(robot_model);
+ m_robotModelName = robot_model;
subscribe("localizedpose", m_localizedPose, &NodeWithRobotModel::localizedPoseCallback);
}
@@ -132,15 +132,15 @@
m_kmodel->build(*file);
}
- virtual void loadRobotDescription(const std::string &robot_model)
+ virtual void loadRobotDescription(void)
{
- if (!robot_model.empty() && robot_model != "-")
+ if (!m_robotModelName.empty() && m_robotModelName != "-")
{
std::string content;
- if (get_param(robot_model, content))
+ if (get_param(m_robotModelName, content))
setRobotDescriptionFromData(content.c_str());
else
- fprintf(stderr, "Robot model '%s' not found!\n", robot_model.c_str());
+ fprintf(stderr, "Robot model '%s' not found!\n", m_robotModelName.c_str());
}
}
@@ -211,6 +211,7 @@
std_msgs::RobotBase2DOdom m_localizedPose;
robot_desc::URDF *m_urdf;
planning_models::KinematicModel *m_kmodel;
+ std::string m_robotModelName;
double m_basePos[3];
};
Modified: pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-14 00:29:03 UTC (rev 3033)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-14 00:46:47 UTC (rev 3034)
@@ -104,7 +104,6 @@
m_displayObstacles = true;
m_checkCollision = false;
- updateODESpaces();
}
~PlanningWorldViewer(void)
@@ -139,7 +138,7 @@
{
ros::Time startTime = ros::Time::now();
printf("Collision: %d [%f s]\n", m_collisionSpace->isCollision(0), (ros::Time::now() - startTime).to_double());
- }
+ }
m_collisionSpace->unlock();
}
}
@@ -235,6 +234,8 @@
void display(void)
{
+ // printf("disp\n");
+
m_displayLock.lock();
m_spaces.displaySpaces();
m_displayLock.unlock();
@@ -318,7 +319,9 @@
ros::init(argc, argv);
viewer = new PlanningWorldViewer(argv[1]);
-
+ viewer->loadRobotDescription();
+ viewer->updateODESpaces();
+
if (viewer->loadedRobot())
{
dsFunctions fn;
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-08-14 00:29:03 UTC (rev 3033)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-14 00:46:47 UTC (rev 3034)
@@ -119,11 +119,11 @@
advertise<std_msgs::PointCloudFloat32>("world_3d_map");
advertise<rostools::Log>("roserr");
- param("max_publish_frequency", m_maxPublishFrequency, 0.5);
- param("retain_pointcloud_duration", m_retainPointcloudDuration, 60.0);
- param("retain_pointcloud_fraction", m_retainPointcloudFraction, 0.02);
- param("retain_above_ground_threshold", m_retainAboveGroundThreshold, 0.01);
- param("verbosity_level", m_verbose, 1);
+ param("world_3d_map/max_publish_frequency", m_maxPublishFrequency, 0.5);
+ param("world_3d_map/retain_pointcloud_duration", m_retainPointcloudDuration, 60.0);
+ param("world_3d_map/retain_pointcloud_fraction", m_retainPointcloudFraction, 0.02);
+ param("world_3d_map/retain_above_ground_threshold", m_retainAboveGroundThreshold, 0.01);
+ param("world_3d_map/verbosity_level", m_verbose, 1);
/* create a thread that does the processing of the input data.
* and one that handles the publishing of the data */
@@ -172,9 +172,12 @@
void baseUpdate(void)
{
planning_node_util::NodeWithRobotModel::baseUpdate();
- int group = m_kmodel->getGroupID(m_urdf->getRobotName() + "::base");
- if (group >= 0)
- m_kmodel->computeTransforms(m_basePos, group);
+ if (m_kmodel)
+ {
+ int group = m_kmodel->getGroupID(m_urdf->getRobotName() + "::base");
+ if (group >= 0)
+ m_kmodel->computeTransforms(m_basePos, group);
+ }
}
void pointCloudCallback(void)
@@ -184,6 +187,8 @@
of data will not happen, but we don't want the node to
postpone processing latest data just because it is not done
with older data. */
+ if (m_verbose)
+ printf("Received laser scan with %u points in frame %d\n", m_inputScan.get_ranges_size(), m_inputScan.header.frame_id);
m_flagMutex.lock();
bool discard = m_working;
@@ -206,7 +211,7 @@
bool success = true;
try
{
- m_tf.transformLaserScanToPointCloud("FRAMEID_MAP", m_toProcess, m_inputScan);
+ m_tf.transformLaserScanToPointCloud("tilt_laser", m_toProcess, m_inputScan);
}
catch(libTF::TransformReference::LookupException& ex)
{
@@ -435,7 +440,10 @@
keep = !m_selfSeeParts[i].body->containsPoint(x, y, z);
if (keep)
+ {
copy->pts[j++] = cloud.pts[k];
+ printf("%f\n", cloud.pts[k].z);
+ }
}
}
if (m_verbose)
@@ -481,9 +489,11 @@
if (argc == 2)
{
- World3DMap map(argv[1]);
- map.spin();
- map.shutdown();
+ World3DMap *map = new World3DMap(argv[1]);
+ map->loadRobotDescription();
+ map->spin();
+ map->shutdown();
+ delete map;
}
else
usage(argv[0]);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
