Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18697] pkg/trunk/motion_planning/ planning_environment
|
From: <is...@us...> - 2009-07-13 21:56:06
|
Revision: 18697
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18697&view=rev
Author: isucan
Date: 2009-07-13 21:56:03 +0000 (Mon, 13 Jul 2009)
Log Message:
-----------
moved the attached bodies monitor from collision to kinematic model
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-13 21:52:41 UTC (rev 18696)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-13 21:56:03 UTC (rev 18697)
@@ -40,9 +40,7 @@
#include "planning_environment/models/collision_models.h"
#include "planning_environment/monitors/kinematic_model_state_monitor.h"
-#include <tf/message_notifier.h>
#include <mapping_msgs/CollisionMap.h>
-#include <mapping_msgs/AttachedObject.h>
#include <boost/thread/mutex.hpp>
@@ -74,8 +72,6 @@
delete collisionMapNotifier_;
if (collisionMapUpdateNotifier_)
delete collisionMapUpdateNotifier_;
- if (attachedBodyNotifier_)
- delete attachedBodyNotifier_;
}
/** \brief Return the instance of the environment model maintained */
@@ -110,12 +106,6 @@
onAfterMapUpdate_ = callback;
}
- /** \brief Define a callback for after updating a map */
- void setOnAfterAttachBodyCallback(const boost::function<void(planning_models::KinematicModel::Link*)> &callback)
- {
- onAfterAttachBody_ = callback;
- }
-
/** \brief Return true if map has been received */
bool haveMap(void) const
{
@@ -141,8 +131,8 @@
void updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear);
void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
void collisionMapUpdateCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
- void attachObjectCallback(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
-
+ virtual bool attachObject(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
+
CollisionModels *cm_;
collision_space::EnvironmentModel *collisionSpace_;
double boxScale_;
@@ -152,11 +142,9 @@
ros::Time lastMapUpdate_;
tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapNotifier_;
tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapUpdateNotifier_;
- tf::MessageNotifier<mapping_msgs::AttachedObject> *attachedBodyNotifier_;
boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onBeforeMapUpdate_;
boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onAfterMapUpdate_;
- boost::function<void(planning_models::KinematicModel::Link*)> onAfterAttachBody_;
};
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-07-13 21:52:41 UTC (rev 18696)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-07-13 21:56:03 UTC (rev 18697)
@@ -40,7 +40,9 @@
#include "planning_environment/models/robot_models.h"
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
+#include <tf/message_notifier.h>
#include <mechanism_msgs/MechanismState.h>
+#include <mapping_msgs/AttachedObject.h>
#include <boost/bind.hpp>
#include <vector>
#include <string>
@@ -77,6 +79,8 @@
{
if (robotState_)
delete robotState_;
+ if (attachedBodyNotifier_)
+ delete attachedBodyNotifier_;
}
/** \brief Define a callback for when the state is changed */
@@ -85,6 +89,12 @@
onStateUpdate_ = callback;
}
+ /** \brief Define a callback for after updating a map */
+ void setOnAfterAttachBodyCallback(const boost::function<void(planning_models::KinematicModel::Link*)> &callback)
+ {
+ onAfterAttachBody_ = callback;
+ }
+
/** \brief Get the kinematic model that is being monitored */
planning_models::KinematicModel* getKinematicModel(void) const
{
@@ -163,8 +173,9 @@
void setupRSM(void);
void mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState);
+ void attachObjectCallback(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
+ virtual bool attachObject(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
-
RobotModels *rm_;
bool includePose_;
planning_models::KinematicModel *kmodel_;
@@ -175,6 +186,9 @@
ros::Subscriber mechanismStateSubscriber_;
tf::TransformListener *tf_;
+ tf::MessageNotifier<mapping_msgs::AttachedObject>
+ *attachedBodyNotifier_;
+
/** \brief How long to wait for a TF if it is not yet available, before failing */
ros::Duration tfWait_;
@@ -184,6 +198,8 @@
std::string frame_id_;
boost::function<void(void)> onStateUpdate_;
+ boost::function<void(planning_models::KinematicModel::Link*)>
+ onAfterAttachBody_;
bool havePose_;
bool haveMechanismState_;
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-13 21:52:41 UTC (rev 18696)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-13 21:56:03 UTC (rev 18697)
@@ -35,7 +35,6 @@
/** \author Ioan Sucan */
#include "planning_environment/monitors/collision_space_monitor.h"
-#include "planning_environment/util/construct_object.h"
#include <robot_msgs/PoseStamped.h>
#include <boost/bind.hpp>
#include <sstream>
@@ -228,59 +227,26 @@
onAfterMapUpdate_(collisionMap);
}
-void planning_environment::CollisionSpaceMonitor::attachObjectCallback(const mapping_msgs::AttachedObjectConstPtr &attachedObject)
+bool planning_environment::CollisionSpaceMonitor::attachObject(const mapping_msgs::AttachedObjectConstPtr &attachedObject)
{
collisionSpace_->lock();
- planning_models::KinematicModel::Link *link = kmodel_->getLink(attachedObject->link_name);
-
- if (attachedObject->objects.size() != attachedObject->poses.size())
- ROS_ERROR("Number of objects to attach does not match number of poses");
- else
- if (link)
- {
- // clear the previously attached bodies
- for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
- delete link->attachedBodies[i];
- link->attachedBodies.resize(0);
- unsigned int n = attachedObject->objects.size();
-
- // create the new ones
- for (unsigned int i = 0 ; i < n ; ++i)
- {
- robot_msgs::PoseStamped pose;
- robot_msgs::PoseStamped poseP;
- pose.pose = attachedObject->poses[i];
- pose.header = attachedObject->header;
- bool err = false;
- try
- {
- tf_->transformPose(attachedObject->link_name, pose, poseP);
- }
- catch(...)
- {
- err = true;
- ROS_ERROR("Unable to transform object to be attached from frame '%s' to frame '%s'", attachedObject->header.frame_id.c_str(), attachedObject->link_name.c_str());
- }
- if (err)
- continue;
-
- shapes::Shape *shape = construct_object(attachedObject->objects[i]);
- if (!shape)
- continue;
-
- unsigned int j = link->attachedBodies.size();
- link->attachedBodies.push_back(new planning_models::KinematicModel::AttachedBody());
- tf::poseMsgToTF(poseP.pose, link->attachedBodies[j]->attachTrans);
- link->attachedBodies[j]->shape = shape;
- }
-
- // update the collision model
- collisionSpace_->updateAttachedBodies();
- ROS_DEBUG("Link '%s' has %d objects attached", attachedObject->link_name.c_str(), n);
- }
- else
- ROS_WARN("Unable to attach object to link '%s'", attachedObject->link_name.c_str());
+ // call the same code as in the kinematic model state monitor, but disable the callback
+ boost::function<void(planning_models::KinematicModel::Link*)> backup = onAfterAttachBody_;
+ onAfterAttachBody_ = NULL;
+ bool result = KinematicModelStateMonitor::attachObject(attachedObject);
+
+ // restore the callback
+ onAfterAttachBody_ = backup;
+ if (result)
+ {
+ collisionSpace_->updateAttachedBodies();
+ ROS_DEBUG("Attached bodies have been updated");
+ }
collisionSpace_->unlock();
- if (link && (onAfterAttachBody_ != NULL))
- onAfterAttachBody_(link);
+
+ // call the event, if needed
+ if (result && (onAfterAttachBody_ != NULL))
+ onAfterAttachBody_(kmodel_->getLink(attachedObject->link_name));
+
+ return result;
}
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-07-13 21:52:41 UTC (rev 18696)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-07-13 21:56:03 UTC (rev 18697)
@@ -35,6 +35,7 @@
/** \author Ioan Sucan */
#include "planning_environment/monitors/kinematic_model_state_monitor.h"
+#include "planning_environment/util/construct_object.h"
#include <angles/angles.h>
#include <sstream>
@@ -184,6 +185,69 @@
onStateUpdate_();
}
+bool planning_environment::KinematicModelStateMonitor::attachObject(const mapping_msgs::AttachedObjectConstPtr &attachedObject)
+{
+ bool result = false;
+ planning_models::KinematicModel::Link *link = kmodel_->getLink(attachedObject->link_name);
+
+ if (attachedObject->objects.size() != attachedObject->poses.size())
+ ROS_ERROR("Number of objects to attach does not match number of poses");
+ else
+ if (link)
+ {
+ // clear the previously attached bodies
+ for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
+ delete link->attachedBodies[i];
+ link->attachedBodies.resize(0);
+ unsigned int n = attachedObject->objects.size();
+
+ // create the new ones
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ robot_msgs::PoseStamped pose;
+ robot_msgs::PoseStamped poseP;
+ pose.pose = attachedObject->poses[i];
+ pose.header = attachedObject->header;
+ bool err = false;
+ try
+ {
+ tf_->transformPose(attachedObject->link_name, pose, poseP);
+ }
+ catch(...)
+ {
+ err = true;
+ ROS_ERROR("Unable to transform object to be attached from frame '%s' to frame '%s'", attachedObject->header.frame_id.c_str(), attachedObject->link_name.c_str());
+ }
+ if (err)
+ continue;
+
+ shapes::Shape *shape = construct_object(attachedObject->objects[i]);
+ if (!shape)
+ continue;
+
+ unsigned int j = link->attachedBodies.size();
+ link->attachedBodies.push_back(new planning_models::KinematicModel::AttachedBody());
+ tf::poseMsgToTF(poseP.pose, link->attachedBodies[j]->attachTrans);
+ link->attachedBodies[j]->shape = shape;
+ }
+
+ result = true;
+ ROS_DEBUG("Link '%s' has %d objects attached", attachedObject->link_name.c_str(), n);
+ }
+ else
+ ROS_WARN("Unable to attach object to link '%s'", attachedObject->link_name.c_str());
+
+ if (result && (onAfterAttachBody_ != NULL))
+ onAfterAttachBody_(link);
+
+ return result;
+}
+
+void planning_environment::KinematicModelStateMonitor::attachObjectCallback(const mapping_msgs::AttachedObjectConstPtr &attachedObject)
+{
+ attachObject(attachedObject);
+}
+
void planning_environment::KinematicModelStateMonitor::waitForState(void) const
{
int s = 0;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
