Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16674] pkg/trunk
|
From: <is...@us...> - 2009-06-03 21:52:28
|
Revision: 16674
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16674&view=rev
Author: isucan
Date: 2009-06-03 21:52:15 +0000 (Wed, 03 Jun 2009)
Log Message:
-----------
switching to new API
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPConstraintEvaluator.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPSpaceInformation.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/base/execute_plan_to_state_minimal.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/call_plan_to_state.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_position_minimal.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state_minimal.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/collision_space/environment.cpp
pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-06-03 21:52:15 UTC (rev 16674)
@@ -96,58 +96,52 @@
public:
- CollisionSpaceMonitor(ros::Node *node, collision_space::EnvironmentModel *collisionSpace = NULL) : KinematicStateMonitor(node)
+ CollisionSpaceMonitor(void) : KinematicStateMonitor()
{
- if (collisionSpace)
- m_collisionSpace = collisionSpace;
- else
- m_collisionSpace = new collision_space::EnvironmentModelODE();
+ m_collisionSpace = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelODE());
m_collisionSpace->setSelfCollision(true);
+
// hack for having ground plane
m_collisionSpace->addStaticPlane(0.0, 0.0, 1.0, -0.01);
-
- m_node->subscribe("collision_map", m_collisionMap, &CollisionSpaceMonitor::collisionMapCallback, this, 1);
- m_node->advertiseService("set_collision_state", &CollisionSpaceMonitor::setCollisionState, this);
-
- m_node->subscribe("attach_object", m_attachedObject, &CollisionSpaceMonitor::attachObject, this, 1);
+ collisionSpaceSubscribe();
}
+
+ CollisionSpaceMonitor(boost::shared_ptr<collision_space::EnvironmentModel> collisionSpace) : KinematicStateMonitor()
+ {
+ // use a given collision space
+ m_collisionSpace = collisionSpace;
+ collisionSpaceSubscribe();
+ }
+
virtual ~CollisionSpaceMonitor(void)
{
- if (m_collisionSpace)
- {
- delete m_collisionSpace;
- m_kmodel = NULL;
- }
}
- void attachObject(void);
bool setCollisionState(motion_planning_srvs::CollisionCheckState::Request &req, motion_planning_srvs::CollisionCheckState::Response &res);
- virtual void setRobotDescription(robot_desc::URDF *file);
-
+ virtual void loadRobotDescription(void);
virtual void defaultPosition(void);
bool isMapUpdated(double sec);
protected:
- void addSelfCollisionGroups(unsigned int cid, robot_desc::URDF *model);
+ void collisionSpaceSubscribe(void);
- void collisionMapCallback(void);
+ void attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject);
+ void collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap);
- virtual void beforeWorldUpdate(void);
- virtual void afterWorldUpdate(void);
- virtual void afterAttachBody(planning_models::KinematicModel::Link *link);
+ virtual void beforeWorldUpdate(const robot_msgs::CollisionMapConstPtr &collisionMap);
+ virtual void afterWorldUpdate(const robot_msgs::CollisionMapConstPtr &collisionMap);
+ virtual void afterAttachBody(const robot_msgs::AttachedObjectConstPtr &attachedObject, planning_models::KinematicModel::Link *link);
- robot_msgs::CollisionMap m_collisionMap;
- collision_space::EnvironmentModel *m_collisionSpace;
-
- // add or remove objects to be attached to a link
- robot_msgs::AttachedObject m_attachedObject;
-
- ros::Time m_lastMapUpdate;
+ boost::shared_ptr<collision_space::EnvironmentModel> m_collisionSpace;
+ ros::Subscriber m_collisionMapSubscriber;
+ ros::Subscriber m_attachedObjectSubscriber;
+ ros::ServiceServer m_setCollisionStateService;
+ ros::Time m_lastMapUpdate;
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-06-03 21:52:15 UTC (rev 16674)
@@ -39,9 +39,9 @@
state the robot is currently in.
*/
-#include <ros/node.h>
-#include <ros/time.h>
+#include <ros/ros.h>
#include <ros/console.h>
+#include <boost/shared_ptr.hpp>
#include <urdf/URDF.h>
#include <planning_models/kinematic.h>
@@ -66,7 +66,7 @@
Subscribes to (name/type):
- @b "mechanism_model"/MechanismModel : position for each of the robot's joints
- - @b "odom_combined"/PoseWithCovariance : localized robot pose
+ - @b "localized_pose"/PoseWithCovariance : localized robot pose
Publishes to (name/type):
- None
@@ -92,42 +92,28 @@
{
public:
- planning_models::KinematicModel* getKModel(void) { return m_kmodel; }
- planning_models::KinematicModel::StateParams* getRobotState() { return m_robotState; }
-
- KinematicStateMonitor(ros::Node *node) : m_tf(*node, true, ros::Duration(1))
+
+ KinematicStateMonitor(void) : m_tf(*ros::Node::instance(), true, ros::Duration(1))
{
m_tf.setExtrapolationLimit(ros::Duration().fromSec(10));
- m_urdf = NULL;
- m_kmodel = NULL;
- m_robotState = NULL;
- m_node = node;
- m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
-
+ m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_includeBaseInState = false;
m_haveMechanismState = false;
m_haveBasePos = false;
- m_node->subscribe("mechanism_state", m_mechanismState, &KinematicStateMonitor::mechanismStateCallback, this, 1);
- m_node->subscribe("odom_combined", m_localizedPose, &KinematicStateMonitor::localizedPoseCallback, this, 1);
+ kinematicStateSubscribe();
}
virtual ~KinematicStateMonitor(void)
{
- if (m_urdf)
- delete m_urdf;
- if (m_robotState)
- delete m_robotState;
- if (m_kmodel)
- delete m_kmodel;
}
+ boost::shared_ptr<planning_models::KinematicModel> getKModel(void) const;
+ boost::shared_ptr<planning_models::KinematicModel::StateParams> getRobotState(void) const;
+
void setIncludeBaseInState(bool value);
- void setRobotDescriptionFromData(const char *data);
- void setRobotDescriptionFromFile(const char *filename);
- virtual void setRobotDescription(robot_desc::URDF *file);
virtual void loadRobotDescription(void);
virtual void defaultPosition(void);
@@ -141,38 +127,38 @@
protected:
+ void kinematicStateSubscribe(void);
+
virtual void stateUpdate(void);
virtual void baseUpdate(void);
- void localizedPoseCallback(void);
- void mechanismStateCallback(void);
+ void localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose);
+ void mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState);
+ ros::NodeHandle m_nodeHandle;
+ ros::Subscriber m_mechanismStateSubscriber;
+ ros::Subscriber m_localizedPoseSubscriber;
-
- ros::Node *m_node;
- tf::TransformListener m_tf;
- robot_desc::URDF *m_urdf;
- planning_models::KinematicModel *m_kmodel;
-
+ tf::TransformListener m_tf;
+
+ boost::shared_ptr<robot_desc::URDF> m_urdf;
+ boost::shared_ptr<planning_models::KinematicModel> m_kmodel;
+
// info about the pose; this is not placed in the robot's kinematic state
- bool m_haveBasePos;
- double m_basePos[3];
- tf::Pose m_pose;
- robot_msgs::PoseWithCovariance m_localizedPose;
+ bool m_haveBasePos;
+ double m_basePos[3];
+ tf::Pose m_pose;
// info about the robot's joints
- robot_msgs::MechanismState m_mechanismState;
- bool m_haveMechanismState;
+ bool m_haveMechanismState;
+ boost::shared_ptr<planning_models::KinematicModel::StateParams>
+ m_robotState;
- // the complete robot state
- planning_models::KinematicModel::StateParams *m_robotState;
-
// if this flag is true, the base position is included in the state as well
- bool m_includeBaseInState;
+ bool m_includeBaseInState;
- ros::Time m_lastStateUpdate;
- ros::Time m_lastBaseUpdate;
+ ros::Time m_lastStateUpdate;
+ ros::Time m_lastBaseUpdate;
};
} // kinematic_planning
-
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-06-03 21:52:15 UTC (rev 16674)
@@ -56,7 +56,7 @@
for (unsigned int i = 0 ; i < pc.size() ; ++i)
{
PoseConstraintEvaluator *pce = new PoseConstraintEvaluator();
- pce->use(m_model->kmodel, pc[i]);
+ pce->use(m_model->kmodel.get(), pc[i]);
m_pce.push_back(pce);
// if we have position constraints
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPConstraintEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPConstraintEvaluator.h 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPConstraintEvaluator.h 2009-06-03 21:52:15 UTC (rev 16674)
@@ -40,6 +40,7 @@
#include <planning_models/kinematic.h>
#include <motion_planning_msgs/KinematicConstraints.h>
#include <angles/angles.h>
+#include <boost/shared_ptr.hpp>
#include <iostream>
#include <vector>
@@ -59,7 +60,7 @@
}
virtual void clear(void) = 0;
- virtual bool use(planning_models::KinematicModel *kmodel, const ros::Message *kc) = 0;
+ virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc) = 0;
virtual bool decide(void) const = 0;
virtual void print(std::ostream &out = std::cout) const
{
@@ -76,7 +77,7 @@
m_link = NULL;
}
- virtual bool use(planning_models::KinematicModel *kmodel, const ros::Message *kc)
+ virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc)
{
const motion_planning_msgs::PoseConstraint *pc = dynamic_cast<const motion_planning_msgs::PoseConstraint*>(kc);
if (pc)
@@ -85,7 +86,7 @@
return false;
}
- bool use(planning_models::KinematicModel *kmodel, const motion_planning_msgs::PoseConstraint &pc)
+ bool use(const planning_models::KinematicModel *kmodel, const motion_planning_msgs::PoseConstraint &pc)
{
m_link = kmodel->getLink(pc.robot_link);
m_pc = pc;
@@ -340,7 +341,7 @@
m_kce.clear();
}
- bool use(planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &kc)
+ bool use(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &kc)
{
clear();
bool result = true;
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h 2009-06-03 21:52:15 UTC (rev 16674)
@@ -40,6 +40,7 @@
#include <collision_space/environment.h>
#include <planning_models/kinematic.h>
#include <boost/thread/mutex.hpp>
+#include <boost/shared_ptr.hpp>
#include <string>
namespace kinematic_planning
@@ -52,20 +53,18 @@
{
groupID = -1;
collisionSpaceID = 0;
- collisionSpace = NULL;
- kmodel = NULL;
}
virtual ~RKPModelBase(void)
{
}
- boost::mutex lock;
- collision_space::EnvironmentModel *collisionSpace;
- unsigned int collisionSpaceID;
- planning_models::KinematicModel *kmodel;
- std::string groupName;
- int groupID;
+ boost::mutex lock;
+ boost::shared_ptr<collision_space::EnvironmentModel> collisionSpace;
+ unsigned int collisionSpaceID;
+ boost::shared_ptr<planning_models::KinematicModel> kmodel;
+ std::string groupName;
+ int groupID;
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPSpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPSpaceInformation.h 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPSpaceInformation.h 2009-06-03 21:52:15 UTC (rev 16674)
@@ -39,7 +39,7 @@
#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
#include "kinematic_planning/RKPModelBase.h"
-
+#include <boost/shared_ptr.hpp>
#include <vector>
namespace kinematic_planning
@@ -148,11 +148,11 @@
}
}
- double m_divisions;
- planning_models::KinematicModel *m_kmodel;
- int m_groupID;
- std::vector<int> m_floatingJoints;
- std::vector<int> m_planarJoints;
+ double m_divisions;
+ boost::shared_ptr<planning_models::KinematicModel> m_kmodel;
+ int m_groupID;
+ std::vector<int> m_floatingJoints;
+ std::vector<int> m_planarJoints;
};
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h 2009-06-03 21:52:15 UTC (rev 16674)
@@ -72,7 +72,7 @@
void setPoseConstraints(const std::vector<motion_planning_msgs::PoseConstraint> &kc)
{
- m_kce.use(m_model->kmodel, kc);
+ m_kce.use(m_model->kmodel.get(), kc);
}
void clearConstraints(void)
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -97,12 +97,10 @@
{
public:
- DisplayPlannerCollisionModel(ros::Node *node) : CollisionSpaceMonitor(node)
+ DisplayPlannerCollisionModel(void) : CollisionSpaceMonitor()
{
- id_ = 0;
-
- m_node->advertise<visualization_msgs::Marker>("visualization_marker", 10240);
- m_node->advertise<robot_msgs::AttachedObject>("attach_object", 1);
+ id_ = 0;
+ visualizationMarkerPublisher_ = m_nodeHandle.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
}
virtual ~DisplayPlannerCollisionModel(void)
@@ -112,28 +110,28 @@
void run(void)
{
loadRobotDescription();
- m_node->spin();
+ ros::spin();
}
protected:
- void afterWorldUpdate(void)
+ void afterWorldUpdate(const robot_msgs::CollisionMapConstPtr &collisionMap)
{
- CollisionSpaceMonitor::afterWorldUpdate();
+ CollisionSpaceMonitor::afterWorldUpdate(collisionMap);
- unsigned int n = m_collisionMap.get_boxes_size();
+ unsigned int n = collisionMap->get_boxes_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- robot_msgs::Point32 &point = m_collisionMap.boxes[i].center;
+ const robot_msgs::Point32 &point = collisionMap->boxes[i].center;
sendPoint(point.x, point.y, point.z,
std::max(std::max(point.x, point.y), point.z),
- m_collisionMap.header, 1);
+ collisionMap->header, 1);
}
}
- void afterAttachBody(planning_models::KinematicModel::Link *link)
+ void afterAttachBody(const robot_msgs::AttachedObjectConstPtr &attachedObject, planning_models::KinematicModel::Link *link)
{
- roslib::Header header = m_attachedObject.header;
+ roslib::Header header = attachedObject->header;
header.frame_id = link->name;
for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
{
@@ -144,6 +142,7 @@
sendPoint(v.x(), v.y(), v.z(), std::max(std::max(box->size[0], box->size[1]), box->size[2] / 2.0), header, 0);
}
}
+ afterAttachBody(attachedObject, link);
}
private:
@@ -179,20 +178,20 @@
mk.color.g = 1.0;
mk.color.b = 0.04;
}
-
- m_node->publish("visualization_marker", mk);
+
+ visualizationMarkerPublisher_.publish(mk);
}
- int id_;
-
+ int id_;
+ ros::Publisher visualizationMarkerPublisher_;
+
};
int main(int argc, char **argv)
{
- ros::init(argc, argv);
+ ros::init(argc, argv, "display_planner_collision_model");
- ros::Node node("display_planner_collision_model");
- DisplayPlannerCollisionModel disp(&node);
+ DisplayPlannerCollisionModel disp;
disp.run();
return 0;
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -38,24 +38,31 @@
namespace kinematic_planning
{
- static inline double radiusOfBox(robot_msgs::Point32 &point)
+ static inline double radiusOfBox(const robot_msgs::Point32 &point)
{
return std::max(std::max(point.x, point.y), point.z) * 1.73;
}
}
-void kinematic_planning::CollisionSpaceMonitor::attachObject(void)
+void kinematic_planning::CollisionSpaceMonitor::collisionSpaceSubscribe(void)
{
+ m_collisionMapSubscriber = m_nodeHandle.subscribe("collision_map", 1, &CollisionSpaceMonitor::collisionMapCallback, this);
+ m_attachedObjectSubscriber = m_nodeHandle.subscribe("attach_object", 1, &CollisionSpaceMonitor::attachObjectCallback, this);
+ m_setCollisionStateService = m_nodeHandle.advertiseService("set_collision_state", &CollisionSpaceMonitor::setCollisionState, this);
+}
+
+void kinematic_planning::CollisionSpaceMonitor::attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject)
+{
m_collisionSpace->lock();
- int model_id = m_collisionSpace->getModelID(m_attachedObject.robot_name);
- planning_models::KinematicModel::Link *link = model_id >= 0 ? m_kmodel->getLink(m_attachedObject.link_name) : NULL;
+ int model_id = m_collisionSpace->getModelID(attachedObject->robot_name);
+ planning_models::KinematicModel::Link *link = model_id >= 0 ? m_kmodel->getLink(attachedObject->link_name) : NULL;
if (link)
{
// clear the previously attached bodies
for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
delete link->attachedBodies[i];
- unsigned int n = m_attachedObject.get_objects_size();
+ unsigned int n = attachedObject->get_objects_size();
link->attachedBodies.resize(n);
// create the new ones
@@ -65,31 +72,31 @@
robot_msgs::PointStamped center;
robot_msgs::PointStamped centerP;
- center.point.x = m_attachedObject.objects[i].center.x;
- center.point.y = m_attachedObject.objects[i].center.y;
- center.point.z = m_attachedObject.objects[i].center.z;
- center.header = m_attachedObject.header;
- m_tf.transformPoint(m_attachedObject.link_name, center, centerP);
+ center.point.x = attachedObject->objects[i].center.x;
+ center.point.y = attachedObject->objects[i].center.y;
+ center.point.z = attachedObject->objects[i].center.z;
+ center.header = attachedObject->header;
+ m_tf.transformPoint(attachedObject->link_name, center, centerP);
link->attachedBodies[i]->attachTrans.setOrigin(btVector3(centerP.point.x, centerP.point.y, centerP.point.z));
// this is a HACK! we should have orientation
planning_models::shapes::Box *box = new planning_models::shapes::Box();
- box->size[0] = m_attachedObject.objects[i].max_bound.x - m_attachedObject.objects[i].min_bound.x;
- box->size[1] = m_attachedObject.objects[i].max_bound.y - m_attachedObject.objects[i].min_bound.y;
- box->size[2] = m_attachedObject.objects[i].max_bound.z - m_attachedObject.objects[i].min_bound.z;
+ box->size[0] = attachedObject->objects[i].max_bound.x - attachedObject->objects[i].min_bound.x;
+ box->size[1] = attachedObject->objects[i].max_bound.y - attachedObject->objects[i].min_bound.y;
+ box->size[2] = attachedObject->objects[i].max_bound.z - attachedObject->objects[i].min_bound.z;
link->attachedBodies[i]->shape = box;
}
// update the collision model
m_collisionSpace->updateAttachedBodies(model_id);
- ROS_INFO("Link '%s' on '%s' has %d objects attached", m_attachedObject.link_name.c_str(), m_attachedObject.robot_name.c_str(), n);
+ ROS_INFO("Link '%s' on '%s' has %d objects attached", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str(), n);
}
else
- ROS_WARN("Unable to attach object to link '%s' on '%s'", m_attachedObject.link_name.c_str(), m_attachedObject.robot_name.c_str());
+ ROS_WARN("Unable to attach object to link '%s' on '%s'", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str());
m_collisionSpace->unlock();
if (link)
- afterAttachBody(link);
+ afterAttachBody(attachedObject, link);
}
bool kinematic_planning::CollisionSpaceMonitor::setCollisionState(motion_planning_srvs::CollisionCheckState::Request &req, motion_planning_srvs::CollisionCheckState::Response &res)
@@ -108,20 +115,26 @@
return true;
}
-void kinematic_planning::CollisionSpaceMonitor::setRobotDescription(robot_desc::URDF *file)
+void kinematic_planning::CollisionSpaceMonitor::loadRobotDescription(void)
{
- KinematicStateMonitor::setRobotDescription(file);
+ KinematicStateMonitor::loadRobotDescription();
if (m_kmodel)
{
std::vector<std::string> links;
- robot_desc::URDF::Group *g = file->getGroup("collision_check");
+ robot_desc::URDF::Group *g = m_urdf->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
+
m_collisionSpace->lock();
unsigned int cid = m_collisionSpace->addRobotModel(m_kmodel, links);
+ std::vector<robot_desc::URDF::Group*> groups;
+ m_urdf->getGroups(groups);
+
+ for (unsigned int i = 0 ; i < groups.size() ; ++i)
+ if (groups[i]->hasFlag("self_collision"))
+ m_collisionSpace->addSelfCollisionGroup(cid, groups[i]->linkNames);
m_collisionSpace->unlock();
- addSelfCollisionGroups(cid, file);
- }
+ }
}
void kinematic_planning::CollisionSpaceMonitor::defaultPosition(void)
@@ -139,35 +152,23 @@
return true;
}
-void kinematic_planning::CollisionSpaceMonitor::addSelfCollisionGroups(unsigned int cid, robot_desc::URDF *model)
+void kinematic_planning::CollisionSpaceMonitor::collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
{
- std::vector<robot_desc::URDF::Group*> groups;
- model->getGroups(groups);
-
- m_collisionSpace->lock();
- for (unsigned int i = 0 ; i < groups.size() ; ++i)
- if (groups[i]->hasFlag("self_collision"))
- m_collisionSpace->addSelfCollisionGroup(cid, groups[i]->linkNames);
- m_collisionSpace->unlock();
-}
-
-void kinematic_planning::CollisionSpaceMonitor::collisionMapCallback(void)
-{
- unsigned int n = m_collisionMap.get_boxes_size();
+ unsigned int n = collisionMap->get_boxes_size();
ROS_DEBUG("Received %u points (collision map)", n);
- beforeWorldUpdate();
+ beforeWorldUpdate(collisionMap);
ros::WallTime startTime = ros::WallTime::now();
double *data = new double[4 * n];
for (unsigned int i = 0 ; i < n ; ++i)
{
unsigned int i4 = i * 4;
- data[i4 ] = m_collisionMap.boxes[i].center.x;
- data[i4 + 1] = m_collisionMap.boxes[i].center.y;
- data[i4 + 2] = m_collisionMap.boxes[i].center.z;
+ data[i4 ] = collisionMap->boxes[i].center.x;
+ data[i4 + 1] = collisionMap->boxes[i].center.y;
+ data[i4 + 2] = collisionMap->boxes[i].center.z;
- data[i4 + 3] = radiusOfBox(m_collisionMap.boxes[i].extents);
+ data[i4 + 3] = radiusOfBox(collisionMap->boxes[i].extents);
}
m_collisionSpace->lock();
@@ -181,18 +182,18 @@
ROS_DEBUG("Updated world model in %f seconds", tupd);
m_lastMapUpdate = ros::Time::now();
- afterWorldUpdate();
+ afterWorldUpdate(collisionMap);
}
-void kinematic_planning::CollisionSpaceMonitor::beforeWorldUpdate(void)
+void kinematic_planning::CollisionSpaceMonitor::beforeWorldUpdate(const robot_msgs::CollisionMapConstPtr &collisionMap)
{
}
-void kinematic_planning::CollisionSpaceMonitor::afterWorldUpdate(void)
+void kinematic_planning::CollisionSpaceMonitor::afterWorldUpdate(const robot_msgs::CollisionMapConstPtr &collisionMap)
{
}
-void kinematic_planning::CollisionSpaceMonitor::afterAttachBody(planning_models::KinematicModel::Link *link)
+void kinematic_planning::CollisionSpaceMonitor::afterAttachBody(const robot_msgs::AttachedObjectConstPtr &attachedObject,
+ planning_models::KinematicModel::Link *link)
{
}
-
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -36,54 +36,49 @@
#include "kinematic_planning/KinematicStateMonitor.h"
-void kinematic_planning::KinematicStateMonitor::setIncludeBaseInState(bool value)
+void kinematic_planning::KinematicStateMonitor::kinematicStateSubscribe(void)
{
- m_includeBaseInState = value;
+ m_mechanismStateSubscriber = m_nodeHandle.subscribe("mechanism_state", 1, &KinematicStateMonitor::mechanismStateCallback, this);
+ m_localizedPoseSubscriber = m_nodeHandle.subscribe("localized_pose", 1, &KinematicStateMonitor::localizedPoseCallback, this);
}
-void kinematic_planning::KinematicStateMonitor::setRobotDescriptionFromData(const char *data)
-{
- robot_desc::URDF *file = new robot_desc::URDF();
- if (file->loadString(data))
- setRobotDescription(file);
- else
- delete file;
+boost::shared_ptr<planning_models::KinematicModel> kinematic_planning::KinematicStateMonitor::getKModel(void) const
+{
+ return m_kmodel;
}
-void kinematic_planning::KinematicStateMonitor::setRobotDescriptionFromFile(const char *filename)
+boost::shared_ptr<planning_models::KinematicModel::StateParams> kinematic_planning::KinematicStateMonitor::getRobotState(void) const
{
- robot_desc::URDF *file = new robot_desc::URDF();
- if (file->loadFile(filename))
- setRobotDescription(file);
- else
- delete file;
+ return m_robotState;
}
-void kinematic_planning::KinematicStateMonitor::setRobotDescription(robot_desc::URDF *file)
+void kinematic_planning::KinematicStateMonitor::setIncludeBaseInState(bool value)
{
- if (m_urdf)
- delete m_urdf;
- if (m_kmodel)
- delete m_kmodel;
-
- m_urdf = file;
- m_kmodel = new planning_models::KinematicModel();
- m_kmodel->setVerbose(false);
- m_kmodel->build(*file);
- m_kmodel->reduceToRobotFrame();
-
- m_robotState = m_kmodel->newStateParams();
- m_robotState->setInRobotFrame();
-
- m_haveMechanismState = false;
- m_haveBasePos = false;
+ m_includeBaseInState = value;
}
void kinematic_planning::KinematicStateMonitor::loadRobotDescription(void)
{
std::string content;
- if (m_node->getParam("robot_description", content))
- setRobotDescriptionFromData(content.c_str());
+ if (m_nodeHandle.getParam("robot_description", content))
+ {
+ m_urdf = boost::shared_ptr<robot_desc::URDF>(new robot_desc::URDF());
+ if (m_urdf->loadString(content.c_str()))
+ {
+ m_kmodel = boost::shared_ptr<planning_models::KinematicModel>(new planning_models::KinematicModel());
+ m_kmodel->setVerbose(false);
+ m_kmodel->build(*m_urdf);
+ m_kmodel->reduceToRobotFrame();
+
+ m_robotState = boost::shared_ptr<planning_models::KinematicModel::StateParams>(m_kmodel->newStateParams());
+ m_robotState->setInRobotFrame();
+
+ m_haveMechanismState = false;
+ m_haveBasePos = false;
+ }
+ else
+ m_urdf.reset();
+ }
else
ROS_ERROR("Robot model not found! Did you remap robot_description?");
}
@@ -96,13 +91,13 @@
bool kinematic_planning::KinematicStateMonitor::loadedRobot(void) const
{
- return m_kmodel != NULL;
+ return m_kmodel;
}
void kinematic_planning::KinematicStateMonitor::waitForState(void)
{
ROS_INFO("Waiting for mechanism state ...");
- while (m_node->ok() && (m_haveMechanismState ^ loadedRobot()))
+ while (m_nodeHandle.ok() && (m_haveMechanismState ^ loadedRobot()))
ros::Duration().fromSec(0.05).sleep();
ROS_INFO("Mechanism state received!");
}
@@ -110,7 +105,7 @@
void kinematic_planning::KinematicStateMonitor::waitForPose(void)
{
ROS_INFO("Waiting for robot pose ...");
- while (m_node->ok() && (m_haveBasePos ^ loadedRobot()))
+ while (m_nodeHandle.ok() && (m_haveBasePos ^ loadedRobot()))
ros::Duration().fromSec(0.05).sleep();
ROS_INFO("Robot pose received!");
}
@@ -160,9 +155,9 @@
stateUpdate();
}
-void kinematic_planning::KinematicStateMonitor::localizedPoseCallback(void)
+void kinematic_planning::KinematicStateMonitor::localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose)
{
- tf::PoseMsgToTF(m_localizedPose.pose, m_pose);
+ tf::PoseMsgToTF(localizedPose->pose, m_pose);
if (std::isfinite(m_pose.getOrigin().x()))
m_basePos[0] = m_pose.getOrigin().x();
if (std::isfinite(m_pose.getOrigin().y()))
@@ -176,35 +171,35 @@
baseUpdate();
}
-void kinematic_planning::KinematicStateMonitor::mechanismStateCallback(void)
+void kinematic_planning::KinematicStateMonitor::mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState)
{
bool change = false;
if (m_robotState)
{
static bool first_time = true;
- unsigned int n = m_mechanismState.get_joint_states_size();
+ unsigned int n = mechanismState->get_joint_states_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- planning_models::KinematicModel::Joint* joint = m_kmodel->getJoint(m_mechanismState.joint_states[i].name);
+ planning_models::KinematicModel::Joint* joint = m_kmodel->getJoint(mechanismState->joint_states[i].name);
if (joint)
{
if (joint->usedParams == 1)
{
- double pos = m_mechanismState.joint_states[i].position;
- bool this_changed = m_robotState->setParamsJoint(&pos, m_mechanismState.joint_states[i].name);
+ double pos = mechanismState->joint_states[i].position;
+ bool this_changed = m_robotState->setParamsJoint(&pos, mechanismState->joint_states[i].name);
change = change || this_changed;
}
else
{
if (first_time)
- ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", m_mechanismState.joint_states[i].name.c_str(), joint->usedParams);
+ ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", mechanismState->joint_states[i].name.c_str(), joint->usedParams);
}
}
else
{
if (first_time)
- ROS_ERROR("Unknown joint: %s", m_mechanismState.joint_states[i].name.c_str());
+ ROS_ERROR("Unknown joint: %s", mechanismState->joint_states[i].name.c_str());
}
}
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -190,10 +190,10 @@
{
public:
- KinematicPlanning(ros::Node *node) : CollisionSpaceMonitor(node)
+ KinematicPlanning(void) : CollisionSpaceMonitor()
{
- m_node->advertiseService("plan_kinematic_path_state", &KinematicPlanning::planToState, this);
- m_node->advertiseService("plan_kinematic_path_position", &KinematicPlanning::planToPosition, this);
+ m_planKinematicPathStateService = m_nodeHandle.advertiseService("plan_kinematic_path_state", &KinematicPlanning::planToState, this);
+ m_planKinematicPathPositionService = m_nodeHandle.advertiseService("plan_kinematic_path_position", &KinematicPlanning::planToPosition, this);
m_replanID = 0;
m_replanningThread = NULL;
@@ -209,17 +209,17 @@
m_currentPlanStatus.unsafe = 0;
m_currentlyExecutedPath.set_states_size(0);
- m_node->advertiseService("replan_kinematic_path_state", &KinematicPlanning::replanToState, this);
- m_node->advertiseService("replan_kinematic_path_position", &KinematicPlanning::replanToPosition, this);
- m_node->advertiseService("replan_force", &KinematicPlanning::forceReplanning, this);
- m_node->advertiseService("replan_stop", &KinematicPlanning::stopReplanning, this);
+ m_replanKinematicPathStateService = m_nodeHandle.advertiseService("replan_kinematic_path_state", &KinematicPlanning::replanToState, this);
+ m_replanKinematicPathPositionService = m_nodeHandle.advertiseService("replan_kinematic_path_position", &KinematicPlanning::replanToPosition, this);
+ m_replanForceService = m_nodeHandle.advertiseService("replan_force", &KinematicPlanning::forceReplanning, this);
+ m_replanStopService = m_nodeHandle.advertiseService("replan_stop", &KinematicPlanning::stopReplanning, this);
- m_node->advertise<motion_planning_msgs::KinematicPlanStatus>("kinematic_planning_status", 1);
+ m_kinematicPlanningPublisher = m_nodeHandle.advertise<motion_planning_msgs::KinematicPlanStatus>("kinematic_planning_status", 1);
// determine intervals; a value of 0 means forever
- m_node->param("refresh_interval_collision_map", m_intervalCollisionMap, 3.0);
- m_node->param("refresh_interval_kinematic_state", m_intervalKinematicState, 0.5);
- m_node->param("refresh_interval_base_pose", m_intervalBasePose, 0.5);
+ m_nodeHandle.param("refresh_interval_collision_map", m_intervalCollisionMap, 3.0);
+ m_nodeHandle.param("refresh_interval_kinematic_state", m_intervalKinematicState, 0.5);
+ m_nodeHandle.param("refresh_interval_base_pose", m_intervalBasePose, 0.5);
}
/** Free the memory */
@@ -244,7 +244,7 @@
startPublishingStatus();
if (mlist.size() > 0)
- m_node->spin();
+ ros::spin();
else
ROS_ERROR("No robot models loaded. OMPL planning node cannot start.");
}
@@ -484,9 +484,9 @@
return result;
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void loadRobotDescription(void)
{
- CollisionSpaceMonitor::setRobotDescription(file);
+ CollisionSpaceMonitor::loadRobotDescription();
defaultPosition();
ROS_DEBUG("=======================================");
@@ -542,7 +542,7 @@
void publishStatus(void)
{
double seconds;
- m_node->param("kinematic_planning_status_interval", seconds, 0.02);
+ m_nodeHandle.param("kinematic_planning_status_interval", seconds, 0.02);
ros::Duration duration(seconds);
ros::Duration delta(std::min(0.01, seconds));
@@ -592,7 +592,7 @@
if (m_currentPlanStatus.path.states.empty())
m_currentPlanStatus.unsafe = isSafeToPlan(false) ? 0 : 1;
- m_node->publish("kinematic_planning_status", m_currentPlanStatus);
+ m_kinematicPlanningPublisher.publish(m_currentPlanStatus);
// if we are sending a new path, remember it
// we will need to check it for validity when the ma changes
@@ -726,9 +726,9 @@
}
/** Event executed after a change in the perceived world is observed */
- virtual void afterWorldUpdate(void)
+ virtual void afterWorldUpdate(const robot_msgs::CollisionMapConstPtr &collisionMap)
{
- CollisionSpaceMonitor::afterWorldUpdate();
+ CollisionSpaceMonitor::afterWorldUpdate(collisionMap);
bool update = false;
// notify the replanning thread of the change
@@ -838,14 +838,13 @@
}
model->addIKKPIECE(options);
}
-
- ModelMap m_models;
-
+
RKPBasicRequest<motion_planning_msgs::KinematicPlanStateRequest> m_requestStateOpenLoop;
RKPBasicRequest<motion_planning_msgs::KinematicPlanLinkPositionRequest> m_requestLinkPositionOpenLoop;
RKPBasicRequest<motion_planning_msgs::KinematicPlanStateRequest> m_requestState;
RKPBasicRequest<motion_planning_msgs::KinematicPlanLinkPositionRequest> m_requestLinkPosition;
+ ModelMap m_models;
// intervals for determining whether the monitored state & map are up to date
double m_intervalCollisionMap;
@@ -853,6 +852,14 @@
double m_intervalBasePose;
+ ros::Publisher m_kinematicPlanningPublisher;
+ ros::ServiceServer m_planKinematicPathStateService;
+ ros::ServiceServer m_planKinematicPathPositionService;
+ ros::ServiceServer m_replanKinematicPathStateService;
+ ros::ServiceServer m_replanKinematicPathPositionService;
+ ros::ServiceServer m_replanForceService;
+ ros::ServiceServer m_replanStopService;
+
/*********** DATA USED FOR REPLANNING ONLY ***********/
// currently considered request
@@ -922,13 +929,12 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv);
+ ros::init(argc, argv, "ompl_planning");
OutputHandlerROScon rosconOutputHandler;
ompl::msg::useOutputHandler(&rosconOutputHandler);
- ros::Node node("ompl_planning");
- KinematicPlanning planner(&node);
+ KinematicPlanning planner;
planner.run();
return 0;
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -101,7 +101,7 @@
class MotionValidator : public CollisionSpaceMonitor
{
-public:
+protected:
class myModel : public RKPModelBase
{
@@ -122,12 +122,14 @@
ompl::sb::SpaceInformationKinematic *si;
ompl::base::StateValidityChecker *svc;
- };
-
- MotionValidator(ros::Node *node) : CollisionSpaceMonitor(node)
+ };
+
+public:
+
+ MotionValidator(void) : CollisionSpaceMonitor()
{
- m_node->advertiseService("validate_direct_path", &MotionValidator::validatePath, this);
- m_node->advertiseService("validate_state", &MotionValidator::validateState, this);
+ m_validateDirectPathService = m_nodeHandle.advertiseService("validate_direct_path", &MotionValidator::validatePath, this);
+ m_validateStateService = m_nodeHandle.advertiseService("validate_state", &MotionValidator::validateState, this);
}
/** Free the memory */
@@ -267,9 +269,9 @@
}
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void loadRobotDescription(void)
{
- CollisionSpaceMonitor::setRobotDescription(file);
+ CollisionSpaceMonitor::loadRobotDescription();
ROS_INFO("=======================================");
std::stringstream ss;
@@ -321,7 +323,7 @@
for (unsigned int i = 0 ; i < mlist.size() ; ++i)
ROS_INFO(" * %s", mlist[i].c_str());
if (mlist.size() > 0)
- m_node->spin();
+ ros::spin();
else
ROS_ERROR("No robot models defined. Path validation node cannot start.");
}
@@ -336,16 +338,17 @@
}
std::map<std::string, myModel*> m_models;
-
+
+ ros::ServiceServer m_validateDirectPathService;
+ ros::ServiceServer m_validateStateService;
};
int main(int argc, char **argv)
{
- ros::init(argc, argv);
+ ros::init(argc, argv, "motion_validator");
- ros::Node node("motion_validator");
- MotionValidator validator(&node);
+ MotionValidator validator;
validator.run();
return 0;
Modified: pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -98,10 +98,10 @@
{
public:
- StateValidityMonitor(ros::Node *node) : CollisionSpaceMonitor(node),
- last_(-1)
+ StateValidityMonitor(void) : CollisionSpaceMonitor(),
+ last_(-1)
{
- m_node->advertise<std_msgs::Byte>("state_validity", 1);
+ stateValidityPublisher_ = m_nodeHandle.advertise<std_msgs::Byte>("state_validity", 1);
}
virtual ~StateValidityMonitor(void)
@@ -112,17 +112,17 @@
{
loadRobotDescription();
waitForState();
- m_node->spin();
+ ros::spin();
}
protected:
- void afterWorldUpdate(void)
+ void afterWorldUpdate(const robot_msgs::CollisionMapConstPtr &collisionMap)
{
- CollisionSpaceMonitor::afterWorldUpdate();
+ CollisionSpaceMonitor::afterWorldUpdate(collisionMap);
last_ = -1;
}
-
+
void stateUpdate(void)
{
CollisionSpaceMonitor::stateUpdate();
@@ -139,7 +139,7 @@
if (last_ != msg.data)
{
last_ = msg.data;
- m_node->publish("state_validity", msg);
+ stateValidityPublisher_.publish(msg);
if (invalid)
ROS_WARN("State is in collision");
else
@@ -150,16 +150,16 @@
private:
- int last_;
-
+ int last_;
+ ros::Publisher stateValidityPublisher_;
+
};
int main(int argc, char **argv)
{
- ros::init(argc, argv);
+ ros::init(argc, argv, "state_validity_monitor");
- ros::Node node("state_validity_monitor");
- StateValidityMonitor validator(&node);
+ StateValidityMonitor validator;
validator.run();
return 0;
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/base/execute_plan_to_state_minimal.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/base/execute_plan_to_state_minimal.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/base/execute_plan_to_state_minimal.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -56,7 +56,7 @@
{
public:
- Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ Example(void) : kinematic_planning::KinematicStateMonitor()
{
}
@@ -99,7 +99,9 @@
motion_planning_srvs::KinematicPlanState::Response s_res;
s_req.value = req;
- if (ros::service::call("plan_kinematic_path_state", s_req, s_res))
+ ros::ServiceClient client = m_nodeHandle.serviceClient<motion_planning_srvs::KinematicPlanState>("plan_kinematic_path_state");
+
+ if (client.call(s_req, s_res))
{
// send command to the base
// no idea how to interface with that yet ...
@@ -126,10 +128,9 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv);
+ ros::init(argc, argv, "example_execute_plan_to_state_minimal");
- ros::Node node("example_execute_plan_to_state_minimal");
- Example plan(&node);
+ Example plan;
plan.run();
return 0;
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -66,19 +66,19 @@
{
public:
- PlanKinematicPath(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ PlanKinematicPath(void) : kinematic_planning::KinematicStateMonitor()
{
plan_id_ = -1;
robot_stopped_ = true;
// we use the topic for sending commands to the controller, so we need to advertise it
- m_node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_command", 1);
- m_node->advertise<robot_msgs::PoseStamped>("cartesian_trajectory/command", 1);
+ jointCommandPublisher_ = m_nodeHandle.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
+ cartesianCommandPublisher_ = m_nodeHandle.advertise<robot_msgs::PoseStamped>("right_arm/?/command", 1);
// advertise the topic for displaying kinematic plans
- m_node->advertise<motion_planning_msgs::DisplayKinematicPath>("display_kinematic_path", 10);
+ displayKinematicPathPublisher_ = m_nodeHandle.advertise<motion_planning_msgs::DisplayKinematicPath>("display_kinematic_path", 10);
- m_node->subscribe("kinematic_planning_status", plan_status_, &PlanKinematicPath::receiveStatus, this, 1);
+ kinematicPlanningStatusSubscriber_ = m_nodeHandle.subscribe("kinematic_planning_status", 1, &PlanKinematicPath::receiveStatus, this);
}
void runRightArmToPositionA(void)
@@ -109,8 +109,9 @@
motion_planning_srvs::KinematicReplanState::Request s_req;
motion_planning_srvs::KinematicReplanState::Response s_res;
s_req.value = req;
-
- if (ros::service::call("replan_kinematic_path_state", s_req, s_res))
+
+ ros::ServiceClient client = m_nodeHandle.serviceClient<motion_planning_srvs::KinematicReplanState>("replan_kinematic_path_state");
+ if (client.call(s_req, s_res))
plan_id_ = s_res.id;
else
ROS_ERROR("Service 'replan_kinematic_path_state' failed");
@@ -153,7 +154,9 @@
motion_planning_srvs::KinematicReplanLinkPosition::Response s_res;
s_req.value = req;
- if (ros::service::call("replan_kinematic_path_position", s_req, s_res))
+ ros::ServiceClient client = m_nodeHandle.serviceClient<motion_planning_srvs::KinematicReplanLinkPosition>("replan_kinematic_path_position");
+
+ if (client.call(s_req, s_res))
plan_id_ = s_res.id;
else
ROS_ERROR("Service 'replan_kinematic_path_position' failed");
@@ -171,7 +174,7 @@
ps.pose.orientation.y = 0;
ps.pose.orientation.z = 0;
ps.pose.orientation.w = 1;
- m_node->publish("cartesian_trajectory/command", ps);
+ cartesianCommandPublisher_.publish(ps);
}
void run(void)
@@ -182,7 +185,7 @@
ros::Duration d(1.0);
d.sleep();
- while (m_node->ok())
+ while (m_nodeHandle.ok())
{
ros::Duration d(10.0);
@@ -201,28 +204,28 @@
plan->runRightArmToCoordinates();
*/
- m_node->spin();
+ ros::spin();
}
}
protected:
// handle new status message
- void receiveStatus(void)
+ void receiveStatus(const motion_planning_msgs::KinematicPlanStatusConstPtr &planStatus)
{
- if (plan_id_ >= 0 && plan_status_.id == plan_id_)
+ if (plan_id_ >= 0 && planStatus->id == plan_id_)
{
- if (plan_status_.valid && !plan_status_.unsafe)
+ if (planStatus->valid && !planStatus->unsafe)
{
- if (!plan_status_.path.states.empty())
+ if (!planStatus->path.states.empty())
{
robot_stopped_ = false;
- sendArmCommand(plan_status_.path, GROUPNAME);
+ sendArmCommand(planStatus->path, GROUPNAME);
}
}
else
stopRobot();
- if (plan_status_.done)
+ if (planStatus->done)
{
plan_id_ = -1;
robot_stopped_ = true;
@@ -259,7 +262,7 @@
}
// convert a kinematic path message to a trajectory for the controller
- void getTrajectoryMsg(motion_planning_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
+ void getTrajectoryMsg(const motion_planning_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
{
traj.set_points_size(path.get_states_size());
for (unsigned int i = 0 ; i < path.get_states_size() ; ++i)
@@ -272,28 +275,28 @@
}
// send a command to the trajectory controller using a topic
- void sendArmCommand(motion_planning_msgs::KinematicPath &path, const std::string &model)
+ void sendArmCommand(const motion_planning_msgs::KinematicPath &path, const std::string &model)
{
sendDisplay(path, model);
printPath(path);
robot_msgs::JointTraj traj;
getTrajectoryMsg(path, traj);
- m_node->publish("right_arm_trajectory_command", traj);
+ jointCommandPublisher_.publish(traj);
ROS_INFO("Sent trajectory to controller");
}
- void sendDisplay(motion_planning_msgs::KinematicPath &path, const std::string &model)
+ void sendDisplay(const motion_planning_msgs::KinematicPath &path, const std::string &model)
{
motion_planning_msgs::DisplayKinematicPath dpath;
dpath.frame_id = "base_link";
dpath.model_name = model;
currentState(dpath.start_state);
dpath.path = path;
- m_node->publish("display_kinematic_path", dpath);
+ displayKinematicPathPublisher_.publish(dpath);
ROS_INFO("Sent planned path to display");
}
- void printPath(motion_planning_msgs::KinematicPath &path)
+ void printPath(const motion_planning_msgs::KinematicPath &path)
{
printf("Path with %d states\n", (int)path.states.size());
for (unsigned int i = 0 ; i < path.states.size() ; ++i)
@@ -305,19 +308,22 @@
printf("\n");
}
- motion_planning_msgs::KinematicPlanStatus plan_status_;
int plan_id_;
bool robot_stopped_;
-
+
+ ros::Publisher jointCommandPublisher_;
+ ros::Publisher cartesianCommandPublisher_;
+ ros::Publisher displayKinematicPathPublisher_;
+ ros::Subscriber kinematicPlanningStatusSubscriber_;
+
};
int main(int argc, char **argv)
{
- ros::init(argc, argv);
- ros::Node *node = new ros::Node("plan_kinematic_path");
-
- PlanKinematicPath plan(node);
+ ros::init(argc, argv, "plan_kinematic_path");
+
+ PlanKinematicPath plan;
plan.run();
return 0;
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/call_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/call_plan_to_state.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/call_plan_to_state.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -50,10 +50,10 @@
{
public:
- Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ Example(void) : kinematic_planning::KinematicStateMonitor()
{
// we use the topic for sending commands to the controller, so we need to advertise it
- m_node->advertise<pr2_msgs::MoveArmGoal>("right_arm_goal", 1);
+ rightArmGoal_ = m_nodeHandle.advertise<pr2_msgs::MoveArmGoal>("right_arm_goal", 1);
}
void runExample(void)
@@ -66,7 +66,7 @@
ag.set_goal_configuration_size(1);
ag.goal_configuration[0].name = "r_shoulder_pan_joint";
ag.goal_configuration[0].position = -0.5;
- m_node->publish("right_arm_goal", ag);
+ rightArmGoal_.publish(ag);
}
void run(void)
@@ -79,16 +79,19 @@
}
sleep(1);
}
+
+private:
+ ros::Publisher rightArmGoal_;
+
};
int main(int argc, char **argv)
{
- ros::init(argc, argv);
+ ros::init(argc, argv, "example_call_plan_to_state");
- ros::Node node("example_call_plan_to_state");
- Example plan(&node);
+ Example plan;
plan.run();
return 0;
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_position_minimal.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_position_minimal.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_position_minimal.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -53,10 +53,10 @@
{
public:
- Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ Example(void) : kinematic_planning::KinematicStateMonitor()
{
// we use the topic for sending commands to the controller, so we need to advertise it
- m_node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_command", 1);
+ jointCommandPublisher_ = m_nodeHandle.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
}
void runExample(void)
@@ -98,11 +98,12 @@
motion_planning_srvs::KinematicPlanLinkPosition::Request s_req;
motion_planning_srvs::KinematicPlanLinkPosition::Response s_res;
s_req.value = req;
-
- if (ros::service::call("plan_kinematic_path_position", s_req, s_res))
+
+ ros::ServiceClient client = m_nodeHandle.serviceClient<motion_planning_srvs::KinematicPlanLinkPosition>("plan_kinematic_path_position");
+ if (client.call(s_req, s_res))
sendArmCommand(s_res.value.path, GROUPNAME);
else
- ROS_ERROR("Service 'plan_kinematic_path_state' failed");
+ ROS_ERROR("Service 'plan_kinematic_path_position' failed");
}
void run(void)
@@ -136,22 +137,22 @@
{
robot_msgs::JointTraj traj;
getTrajectoryMsg(path, traj);
- m_node->publish("right_arm_trajectory_command", traj);
+ jointCommandPublisher_.publish(traj);
ROS_INFO("Sent trajectory to controller (using a topic)");
}
private:
+ ros::Publisher jointCommandPublisher_;
};
int main(int argc, char **argv)
{
- ros::init(argc, argv);
+ ros::init(argc, argv, "example_execute_plan_to_position_minimal");
- ros::Node node("example_execute_plan_to_position_minimal");
- Example plan(&node);
+ Example plan;
plan.run();
return 0;
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state.cpp 2009-06-03 21:23:48 UTC (rev 16673)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state.cpp 2009-06-03 21:52:15 UTC (rev 16674)
@@ -71,13 +71,13 @@
{
public:
- Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ Example(void) : kinematic_planning::KinematicStateMonitor()
{
// if we use the topic for sending commands to the controller, we need to advertise it
- m_node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_command", 1);
+ jointCommandPublisher_ = m_nodeHandle.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
// advertise the topic for displaying kinematic plans
- m_node->advertise<motion_planning_msgs::DisplayKinematicPath>("display_kinematic_path", 10);
+ displayKinematicPathPublisher_ = m_nodeHandle.advertise<motion_planning_msgs::DisplayKinematicPath>("display_kinematic_path", 10);
// we can send commands to the trajectory controller both by using a topic, and by using services
use_topic_ = false;
@@ -131,8 +131,9 @@
motion_planning_srvs::KinematicPlanState::Request s_req;
motion_planning_srvs::KinematicPlanState::Response s_res;
s_req.value = req;
-
- if (ros::service::call("plan_kinematic_path_state", s_req, s_res))
+
+ ros::ServiceClient client = m_nodeHandle.serviceClient<motion_planning_srvs::KinematicPlanState>("plan_kinematic_path_state");
+ if (client.call(s_req, s_res))
{
// print the path on screen
printPath(s_res.value.path);
@@ -175,7 +176,7 @@
}
// convert a kinematic path message to a trajectory for the controller
- void getTrajectoryMsg(motion_planning_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
+ void getTrajectoryMsg(const motion_planning_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
{
traj.set_points_size(path.get_states_size());
for (unsigned int i = 0 ; i < path.get_states_size() ; ++i)
@@ -188,17 +189,17 @@
}
// send a command to the trajectory controller using a topic
- void sendArmCommand(motion_planning_msgs::KinematicPath &path, const std::string &model)
+ void sendArmCommand(const motion_planning_msgs::KinematicPath &path, const std::string &model)
{
robot_msgs::JointTr...
[truncated message content] |
