Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3020] pkg/trunk
|
From: <is...@us...> - 2008-08-13 23:22:05
|
Revision: 3020
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3020&view=rev
Author: isucan
Date: 2008-08-13 23:22:13 +0000 (Wed, 13 Aug 2008)
Log Message:
-----------
switched planning_world_viewer to use planning_node_util
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/motion_planning/planning_world_viewer/manifest.xml
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h
Added Paths:
-----------
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
Copied: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h (from rev 3013, pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h)
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h (rev 0)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-13 23:22:13 UTC (rev 3020)
@@ -0,0 +1,119 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <planning_node_util/knode.h>
+#include <std_msgs/PointCloudFloat32.h>
+#include <collision_space/environmentODE.h>
+
+namespace planning_node_util
+{
+
+ class NodeWithODECollisionModel : public NodeWithRobotModel
+ {
+
+ public:
+
+ NodeWithODECollisionModel(const std::string &robot_model, const std::string &name, uint32_t options = 0) : NodeWithRobotModel(robot_model, name, options)
+ {
+ m_collisionSpace = new collision_space::EnvironmentModelODE();
+ m_collisionSpace->setSelfCollision(false);
+ m_sphereSize = 0.03;
+
+ subscribe("world_3d_map", m_worldCloud, &NodeWithODECollisionModel::worldMapCallback);
+ }
+
+ virtual ~NodeWithODECollisionModel(void)
+ {
+ if (m_collisionSpace)
+ delete m_collisionSpace;
+ }
+
+ virtual void setRobotDescription(robot_desc::URDF *file)
+ {
+ NodeWithRobotModel::setRobotDescription(file);
+ if (m_kmodel)
+ {
+ m_collisionSpace->lock();
+ m_collisionSpace->addRobotModel(m_kmodel);
+ m_collisionSpace->unlock();
+ }
+ }
+
+ protected:
+
+ std_msgs::PointCloudFloat32 m_worldCloud;
+ collision_space::EnvironmentModelODE *m_collisionSpace;
+ double m_sphereSize;
+
+ void worldMapCallback(void)
+ {
+ unsigned int n = m_worldCloud.get_pts_size();
+ printf("received %u points (world map)\n", n);
+
+ beforeWorldUpdate();
+
+ ros::Time startTime = ros::Time::now();
+ double *data = new double[3 * n];
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ unsigned int i3 = i * 3;
+ data[i3 ] = m_worldCloud.pts[i].x;
+ data[i3 + 1] = m_worldCloud.pts[i].y;
+ data[i3 + 2] = m_worldCloud.pts[i].z;
+ }
+
+ m_collisionSpace->lock();
+ m_collisionSpace->clearObstacles();
+ m_collisionSpace->addPointCloud(n, data, m_sphereSize);
+ m_collisionSpace->unlock();
+
+ delete[] data;
+
+ double tupd = (ros::Time::now() - startTime).to_double();
+ printf("Updated world model in %f seconds\n", tupd);
+
+ afterWorldUpdate();
+ }
+
+ virtual void beforeWorldUpdate(void)
+ {
+ }
+
+ virtual void afterWorldUpdate(void)
+ {
+ }
+
+ };
+
+}
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-13 23:18:57 UTC (rev 3019)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-08-13 23:22:13 UTC (rev 3020)
@@ -109,6 +109,23 @@
}
}
+ void defaultPosition(void)
+ {
+ if (m_kmodel)
+ {
+ double defaultPose[m_kmodel->stateDimension];
+ for (unsigned int i = 0 ; i < m_kmodel->stateDimension ; ++i)
+ defaultPose[i] = 0.0;
+
+ m_kmodel->computeTransforms(defaultPose);
+ }
+ }
+
+ bool loadedRobot(void) const
+ {
+ return m_kmodel != NULL;
+ }
+
protected:
void localizedPoseCallback(void)
Modified: pkg/trunk/motion_planning/planning_world_viewer/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/manifest.xml 2008-08-13 23:18:57 UTC (rev 3019)
+++ pkg/trunk/motion_planning/planning_world_viewer/manifest.xml 2008-08-13 23:22:13 UTC (rev 3020)
@@ -5,7 +5,7 @@
<depend package="roscpp" />
<depend package="robot_msgs" />
<depend package="robot_srvs" />
- <depend package="xmlparam" />
- <depend package="collision_space" />
+ <depend package="rosthread" />
+ <depend package="planning_node_util" />
<depend package="display_ode_spaces" />
</package>
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-13 23:18:57 UTC (rev 3019)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-13 23:22:13 UTC (rev 3020)
@@ -81,15 +81,9 @@
**/
-#include <ros/node.h>
-#include <ros/time.h>
-#include <std_msgs/PointCloudFloat32.h>
-#include <std_msgs/RobotBase2DOdom.h>
-
+#include <planning_node_util/cnode.h>
+#include <rosthread/mutex.h>
#include <robot_msgs/NamedKinematicPath.h>
-
-#include <urdf/URDF.h>
-#include <collision_space/environmentODE.h>
#include <display_ode/displayODE.h>
#include <vector>
@@ -97,77 +91,44 @@
#include <sstream>
#include <map>
-class PlanningWorldViewer : public ros::node
+class PlanningWorldViewer : public planning_node_util::NodeWithODECollisionModel
{
public:
- PlanningWorldViewer(void) : ros::node("planning_world_viewer")
+ PlanningWorldViewer(const std::string &robot_model) : planning_node_util::NodeWithODECollisionModel(robot_model, "planning_world_viewer")
{
- subscribe("world_3d_map", m_cloud, &PlanningWorldViewer::pointCloudCallback);
- subscribe("localizedpose", m_localizedPose, &PlanningWorldViewer::localizedPoseCallback);
subscribe("display_kinematic_path", m_displayPath, &PlanningWorldViewer::displayPathCallback);
- m_collisionSpace = new collision_space::EnvironmentModelODE();
- m_collisionSpace->setSelfCollision(false);
- m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_follow = true;
m_displayRobot = true;
m_displayObstacles = true;
m_checkCollision = false;
- m_collisionSpace->lock();
- loadRobotDescriptions();
- m_collisionSpace->unlock();
-
updateODESpaces();
}
~PlanningWorldViewer(void)
{
- for (unsigned int i = 0 ; i < m_robotDescriptions.size() ; ++i)
- delete m_robotDescriptions[i];
- if (m_collisionSpace)
- delete m_collisionSpace;
}
void updateODESpaces(void)
{
- m_collisionSpace->lock();
+ m_collisionSpace->lock();
+ m_displayLock.lock();
m_spaces.clear();
if (m_displayObstacles)
m_spaces.addSpace(m_collisionSpace->getODESpace(), 1.0f, 0.0f, 0.0f);
if (m_displayRobot)
for (unsigned int i = 0 ; i < m_collisionSpace->getModelCount() ; ++i)
m_spaces.addSpace(m_collisionSpace->getModelODESpace(i), 0.1f, 0.5f, (float)(i + 1)/(float)m_collisionSpace->getModelCount());
+ m_displayLock.unlock();
m_collisionSpace->unlock();
}
- void loadRobotDescriptions(void)
+ void baseUpdate(void)
{
- printf("Loading robot descriptions...\n\n");
+ planning_node_util::NodeWithODECollisionModel::baseUpdate();
- std::string description_files;
- if (get_param("robotdesc_list", description_files))
- {
- std::stringstream sdf(description_files);
- while (sdf.good())
- {
- std::string file;
- std::string content;
- sdf >> file;
- if (get_param(file, content))
- addRobotDescriptionFromData(content.c_str());
- }
- }
- printf("\n\n");
- }
-
- void localizedPoseCallback(void)
- {
- m_basePos[0] = m_localizedPose.pos.x;
- m_basePos[1] = m_localizedPose.pos.y;
- m_basePos[2] = m_localizedPose.pos.th;
-
if (m_collisionSpace && m_collisionSpace->getModelCount() == 1 && m_follow)
{
int group = m_collisionSpace->getModel(0)->getGroupID("pr2::base");
@@ -183,113 +144,43 @@
}
}
- void pointCloudCallback(void)
+ virtual void beforeWorldUpdate(void)
{
- unsigned int n = m_cloud.get_pts_size();
- printf("received %u points\n", n);
-
- ros::Time startTime = ros::Time::now();
- double *data = new double[3 * n];
- for (unsigned int i = 0 ; i < n ; ++i)
- {
- unsigned int i3 = i * 3;
- data[i3 ] = m_cloud.pts[i].x;
- data[i3 + 1] = m_cloud.pts[i].y;
- data[i3 + 2] = m_cloud.pts[i].z;
- }
-
- m_collisionSpace->lock();
+ planning_node_util::NodeWithODECollisionModel::beforeWorldUpdate();
+ m_displayLock.lock();
m_spaces.clear();
- m_collisionSpace->clearObstacles();
- m_collisionSpace->addPointCloud(n, data, 0.03);
- m_collisionSpace->unlock();
-
- delete[] data;
-
- updateODESpaces();
-
- double tupd = (ros::Time::now() - startTime).to_double();
- printf("Updated world model in %f seconds\n", tupd);
-
+ m_displayLock.unlock();
}
+
+ virtual void afterWorldUpdate(void)
+ {
+ planning_node_util::NodeWithODECollisionModel::afterWorldUpdate();
+ updateODESpaces();
+ }
void displayPathCallback(void)
{
- if (m_collisionSpace->getModelCount() != 1)
- return;
bool follow = m_follow;
m_follow = false;
ros::Duration sleepTime(0.2);
- planning_models::KinematicModel *kmodel = m_collisionSpace->getModel(0);
- int groupID = kmodel->getGroupID(m_displayPath.name);
+ int groupID = m_kmodel->getGroupID(m_displayPath.name);
for (unsigned int i = 0 ; i < m_displayPath.path.get_states_size() ; ++i)
{
- kmodel->computeTransforms(m_displayPath.path.states[i].vals, groupID);
+ m_kmodel->computeTransforms(m_displayPath.path.states[i].vals, groupID);
m_collisionSpace->updateRobotModel(0);
sleepTime.sleep();
}
m_follow = follow;
}
- void addRobotDescriptionFromFile(const char *filename)
+ virtual void setRobotDescription(robot_desc::URDF *file)
{
- robot_desc::URDF *file = new robot_desc::URDF();
- if (file->loadFile(filename))
- addRobotDescription(file);
- else
- delete file;
+ planning_node_util::NodeWithODECollisionModel::setRobotDescription(file);
+ defaultPosition();
+ m_collisionSpace->updateRobotModel(0);
}
- void addRobotDescriptionFromData(const char *data)
- {
- robot_desc::URDF *file = new robot_desc::URDF();
- if (file->loadString(data))
- addRobotDescription(file);
- else
- delete file;
- }
-
- void addRobotDescription(robot_desc::URDF *file)
- {
- m_robotDescriptions.push_back(file);
-
- printf("\n\nCreating new kinematic model:\n");
-
- /* create a model for the whole robot (with the name given in the file) */
- planning_models::KinematicModel *kmodel = new planning_models::KinematicModel();
- kmodel->setVerbose(true);
- kmodel->build(*file);
-
- /* add the model to the collision space */
- unsigned int cid = m_collisionSpace->addRobotModel(kmodel);
-
- defaultPosition(kmodel, cid);
- }
-
- void defaultPosition(planning_models::KinematicModel *kmodel = NULL, unsigned int cid = 0)
- {
- if (!kmodel)
- {
- if (m_collisionSpace->getModelCount() == 1)
- kmodel = m_collisionSpace->getModel(0);
- else
- return;
- }
-
- double defaultPose[kmodel->stateDimension];
- for (unsigned int i = 0 ; i < kmodel->stateDimension ; ++i)
- defaultPose[i] = 0.0;
-
- kmodel->computeTransforms(defaultPose);
- m_collisionSpace->updateRobotModel(cid);
- }
-
- unsigned int getRobotCount(void) const
- {
- return m_robotDescriptions.size();
- }
-
bool getFollow(void) const
{
return m_follow;
@@ -345,20 +236,16 @@
void display(void)
{
- m_collisionSpace->lock();
+ m_displayLock.lock();
m_spaces.displaySpaces();
- m_collisionSpace->unlock();
+ m_displayLock.unlock();
}
private:
- std_msgs::PointCloudFloat32 m_cloud;
- std_msgs::RobotBase2DOdom m_localizedPose;
- collision_space::EnvironmentModelODE *m_collisionSpace;
- std::vector<robot_desc::URDF*> m_robotDescriptions;
- double m_basePos[3];
-
display_ode::DisplayODESpaces m_spaces;
+ ros::thread::mutex m_displayLock;
+
robot_msgs::NamedKinematicPath m_displayPath;
bool m_follow;
bool m_displayRobot;
@@ -419,30 +306,41 @@
viewer->display();
}
+void usage(const char *progname)
+{
+ printf("\nUsage: %s robot_model [standard ROS args]\n", progname);
+ printf(" \"robot_model\" is the name (string) of a robot description to be used when showing the map.\n");
+}
+
int main(int argc, char **argv)
{
- ros::init(argc, argv);
-
- viewer = new PlanningWorldViewer();
-
- if (viewer->getRobotCount() > 0)
- {
- dsFunctions fn;
- fn.version = DS_VERSION;
- fn.start = &start;
- fn.step = &simLoop;
- fn.command = &command;
- fn.stop = 0;
- fn.path_to_textures = "./res";
+ if (argc == 2)
+ {
+ ros::init(argc, argv);
+
+ viewer = new PlanningWorldViewer(argv[1]);
- dsSimulationLoop(argc, argv, 640, 480, &fn);
+ if (viewer->loadedRobot())
+ {
+ dsFunctions fn;
+ fn.version = DS_VERSION;
+ fn.start = &start;
+ fn.step = &simLoop;
+ fn.command = &command;
+ fn.stop = 0;
+ fn.path_to_textures = "./res";
+
+ dsSimulationLoop(argc, argv, 640, 480, &fn);
+ }
+ else
+ printf("No model defined. Display world node cannot start.\n");
+
+ viewer->shutdown();
+
+ delete viewer;
}
else
- printf("No models defined. Kinematic planning node cannot start.\n");
+ usage(argv[0]);
- viewer->shutdown();
-
- delete viewer;
-
return 0;
}
Modified: pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h
===================================================================
--- pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h 2008-08-13 23:18:57 UTC (rev 3019)
+++ pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h 2008-08-13 23:22:13 UTC (rev 3020)
@@ -32,6 +32,9 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
+#ifndef DISPLAY_ODE_H
+#define DISPLAY_ODE_H
+
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <vector>
@@ -132,3 +135,4 @@
};
}
+#endif
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
