|
From: <is...@us...> - 2008-08-17 21:40:03
|
Revision: 3187
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3187&view=rev
Author: isucan
Date: 2008-08-17 21:40:10 +0000 (Sun, 17 Aug 2008)
Log Message:
-----------
planning_node_util no longer inherits from ROS nodes
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
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/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-17 18:14:13 UTC (rev 3186)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-17 21:40:10 UTC (rev 3187)
@@ -61,6 +61,10 @@
Subscribes to (name/type):
- None
+Additional subscriptions due to inheritance from NodeODECollisionModel:
+- @b localizedpose/RobotBase2DOdom : localized position of the robot base
+- @b world_3d_map/PointCloudFloat32 : point cloud with data describing the 3D environment
+
Publishes to (name/type):
- None
@@ -94,11 +98,13 @@
#include <sstream>
#include <map>
-class KinematicPlanning : public planning_node_util::NodeWithODECollisionModel
+class KinematicPlanning : public ros::node,
+ public planning_node_util::NodeODECollisionModel
{
public:
- KinematicPlanning(const std::string &robot_model) : planning_node_util::NodeWithODECollisionModel(robot_model, "kinematic_planning")
+ KinematicPlanning(const std::string &robot_model) : ros::node("kinematic_planning"),
+ planning_node_util::NodeODECollisionModel(dynamic_cast<ros::node*>(this), robot_model)
{
advertise_service("plan_kinematic_path", &KinematicPlanning::plan);
}
@@ -224,7 +230,7 @@
virtual void setRobotDescription(robot_desc::URDF *file)
{
- planning_node_util::NodeWithODECollisionModel::setRobotDescription(file);
+ planning_node_util::NodeODECollisionModel::setRobotDescription(file);
defaultPosition();
/* set the data for the model */
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-17 18:14:13 UTC (rev 3186)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-17 21:40:10 UTC (rev 3187)
@@ -38,8 +38,8 @@
@htmlinclude ../../manifest.html
-@b NodeWithODECollisionModel is a ROS node that in addition to being aware of a robot model,
-is also aware of an ODE collision space
+@b NodeODECollisionModel is a class which in addition to being aware of a robot model,
+is also aware of an ODE collision space.
<hr>
@@ -48,6 +48,9 @@
Subscribes to (name/type):
- @b world_3d_map/PointCloudFloat32 : point cloud with data describing the 3D environment
+Additional subscriptions due to inheritance from NodeRobotModel:
+- @b localizedpose/RobotBase2DOdom : localized position of the robot base
+
Publishes to (name/type):
- None
@@ -75,21 +78,21 @@
namespace planning_node_util
{
- class NodeWithODECollisionModel : public NodeWithRobotModel
+ class NodeODECollisionModel : public NodeRobotModel
{
public:
- NodeWithODECollisionModel(const std::string &robot_model, const std::string &name, uint32_t options = 0) : NodeWithRobotModel(robot_model, name, options)
+ NodeODECollisionModel(ros::node *node, const std::string &robot_model) : NodeRobotModel(node, robot_model)
{
m_collisionSpace = new collision_space::EnvironmentModelODE();
m_collisionSpace->setSelfCollision(false);
m_sphereSize = 0.03;
- subscribe("world_3d_map", m_worldCloud, &NodeWithODECollisionModel::worldMapCallback);
+ m_node->subscribe("world_3d_map", m_worldCloud, &NodeODECollisionModel::worldMapCallback, this);
}
- virtual ~NodeWithODECollisionModel(void)
+ virtual ~NodeODECollisionModel(void)
{
if (m_collisionSpace)
{
@@ -100,7 +103,7 @@
virtual void setRobotDescription(robot_desc::URDF *file)
{
- NodeWithRobotModel::setRobotDescription(file);
+ NodeRobotModel::setRobotDescription(file);
if (m_kmodel)
{
m_collisionSpace->lock();
@@ -111,7 +114,7 @@
virtual void defaultPosition(void)
{
- NodeWithRobotModel::defaultPosition();
+ NodeRobotModel::defaultPosition();
if (m_collisionSpace && m_collisionSpace->getModelCount() == 1)
m_collisionSpace->updateRobotModel(0);
}
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-17 18:14:13 UTC (rev 3186)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-08-17 21:40:10 UTC (rev 3187)
@@ -38,7 +38,8 @@
@htmlinclude ../../manifest.html
-@b NodeWithRobotModel is a ROS node that is also aware of a given robot model
+@b NodeRobotModel is a class that is also of a given robot model and
+uses a ROS node to retrieve the necessary data.
<hr>
@@ -77,23 +78,23 @@
namespace planning_node_util
{
- class NodeWithRobotModel : public ros::node
+ class NodeRobotModel
{
public:
- NodeWithRobotModel(const std::string &robot_model, const std::string &name, uint32_t options = 0) : ros::node(name, options),
- m_tf(*this, true, 1 * 1000000000ULL, 1000000000ULL)
+ NodeRobotModel(ros::node *node, const std::string &robot_model) : m_tf(*node, true, 1 * 1000000000ULL, 1000000000ULL)
{
m_urdf = NULL;
m_kmodel = NULL;
+ m_node = node;
m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_robotModelName = robot_model;
- subscribe("localizedpose", m_localizedPose, &NodeWithRobotModel::localizedPoseCallback);
+ m_node->subscribe("localizedpose", m_localizedPose, &NodeRobotModel::localizedPoseCallback, this);
}
- virtual ~NodeWithRobotModel(void)
+ virtual ~NodeRobotModel(void)
{
if (m_urdf)
delete m_urdf;
@@ -137,7 +138,7 @@
if (!m_robotModelName.empty() && m_robotModelName != "-")
{
std::string content;
- if (get_param(m_robotModelName, content))
+ if (m_node->get_param(m_robotModelName, content))
setRobotDescriptionFromData(content.c_str());
else
fprintf(stderr, "Robot model '%s' not found!\n", m_robotModelName.c_str());
@@ -208,6 +209,7 @@
}
rosTFClient m_tf;
+ ros::node *m_node;
std_msgs::RobotBase2DOdom m_localizedPose;
robot_desc::URDF *m_urdf;
planning_models::KinematicModel *m_kmodel;
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-17 18:14:13 UTC (rev 3186)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-17 21:40:10 UTC (rev 3187)
@@ -61,6 +61,10 @@
Subscribes to (name/type):
- None
+Additional subscriptions due to inheritance from NodeODECollisionModel:
+- @b localizedpose/RobotBase2DOdom : localized position of the robot base
+- @b world_3d_map/PointCloudFloat32 : point cloud with data describing the 3D environment
+
Publishes to (name/type):
- None
@@ -91,11 +95,13 @@
#include <sstream>
#include <map>
-class PlanningWorldViewer : public planning_node_util::NodeWithODECollisionModel
+class PlanningWorldViewer : public ros::node,
+ public planning_node_util::NodeODECollisionModel
{
public:
- PlanningWorldViewer(const std::string &robot_model) : planning_node_util::NodeWithODECollisionModel(robot_model, "planning_world_viewer")
+ PlanningWorldViewer(const std::string &robot_model) : ros::node("planning_world_viewer"),
+ planning_node_util::NodeODECollisionModel(dynamic_cast<ros::node*>(this), robot_model)
{
subscribe("display_kinematic_path", m_displayPath, &PlanningWorldViewer::displayPathCallback);
@@ -126,7 +132,7 @@
void baseUpdate(void)
{
- planning_node_util::NodeWithODECollisionModel::baseUpdate();
+ planning_node_util::NodeODECollisionModel::baseUpdate();
if (m_collisionSpace && m_collisionSpace->getModelCount() == 1 && m_follow)
{
@@ -145,7 +151,7 @@
virtual void beforeWorldUpdate(void)
{
- planning_node_util::NodeWithODECollisionModel::beforeWorldUpdate();
+ planning_node_util::NodeODECollisionModel::beforeWorldUpdate();
m_displayLock.lock();
m_spaces.clear();
m_displayLock.unlock();
@@ -153,7 +159,7 @@
virtual void afterWorldUpdate(void)
{
- planning_node_util::NodeWithODECollisionModel::afterWorldUpdate();
+ planning_node_util::NodeODECollisionModel::afterWorldUpdate();
updateODESpaces();
}
@@ -175,7 +181,7 @@
virtual void setRobotDescription(robot_desc::URDF *file)
{
- planning_node_util::NodeWithODECollisionModel::setRobotDescription(file);
+ planning_node_util::NodeODECollisionModel::setRobotDescription(file);
defaultPosition();
}
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-17 18:14:13 UTC (rev 3186)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-17 21:40:10 UTC (rev 3187)
@@ -74,6 +74,9 @@
Subscribes to (name/type):
- @b scan/LaserScan : scan data received from a laser
+Additional subscriptions due to inheritance from NodeRobotModel:
+- @b localizedpose/RobotBase2DOdom : localized position of the robot base
+
Publishes to (name/type):
- @b "world_3d_map"/PointCloudFloat32 : point cloud describing the 3D environment
- @b "roserr"/Log : output log messages
@@ -110,11 +113,13 @@
#include <deque>
#include <cmath>
-class World3DMap : public planning_node_util::NodeWithRobotModel
+class World3DMap : public ros::node,
+ public planning_node_util::NodeRobotModel
{
public:
-
- World3DMap(const std::string &robot_model) : planning_node_util::NodeWithRobotModel(robot_model, "world_3d_map")
+
+ World3DMap(const std::string &robot_model) : ros::node("world_3d_map"),
+ planning_node_util::NodeRobotModel(dynamic_cast<ros::node*>(this), robot_model)
{
advertise<std_msgs::PointCloudFloat32>("world_3d_map");
advertise<rostools::Log>("roserr");
@@ -157,7 +162,7 @@
virtual void setRobotDescription(robot_desc::URDF *file)
{
- planning_node_util::NodeWithRobotModel::setRobotDescription(file);
+ planning_node_util::NodeRobotModel::setRobotDescription(file);
addSelfSeeBodies();
}
@@ -171,7 +176,7 @@
void baseUpdate(void)
{
- planning_node_util::NodeWithRobotModel::baseUpdate();
+ planning_node_util::NodeRobotModel::baseUpdate();
if (m_kmodel)
{
int group = m_kmodel->getGroupID(m_urdf->getRobotName() + "::base");
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|