Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16830] pkg/trunk
|
From: <is...@us...> - 2009-06-08 05:49:13
|
Revision: 16830
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16830&view=rev
Author: isucan
Date: 2009-06-08 05:49:11 +0000 (Mon, 08 Jun 2009)
Log Message:
-----------
kinematic state + collision map monitoring in planning_environment
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
pkg/trunk/world_models/collision_space/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-08 05:49:11 UTC (rev 16830)
@@ -81,6 +81,7 @@
while (m_nodeHandle.ok() && (m_haveMechanismState ^ loadedRobot()))
{
ROS_INFO("Waiting for mechanism state ...");
+ ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
}
ROS_INFO("Mechanism state received!");
@@ -88,9 +89,12 @@
void kinematic_planning::KinematicStateMonitor::waitForPose(void)
{
- ROS_INFO("Waiting for robot pose ...");
while (m_nodeHandle.ok() && (m_haveBasePos ^ loadedRobot()))
+ {
+ ROS_INFO("Waiting for robot pose ...");
+ ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
+ }
ROS_INFO("Robot pose received!");
}
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-08 05:49:11 UTC (rev 16830)
@@ -6,7 +6,9 @@
set(ROS_BUILD_TYPE Release)
rospack_add_library(planning_environment src/robot_models.cpp
- src/collision_models.cpp)
+ src/collision_models.cpp
+ src/kinematic_model_state_monitor.cpp
+ src/collision_space_monitor.cpp)
# Tests
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-06-08 05:49:11 UTC (rev 16830)
@@ -39,17 +39,11 @@
#include "planning_environment/robot_models.h"
#include <collision_space/environmentODE.h>
-#include <ros/ros.h>
-#include <boost/shared_ptr.hpp>
namespace planning_environment
{
- /** @htmlinclude ../../manifest.html
-
- @mainpage
-
- A class capable of loading a robot model from the parameter server */
+ /** A class capable of loading a robot model from the parameter server */
class CollisionModels : public RobotModels
{
Added: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-08 05:49:11 UTC (rev 16830)
@@ -0,0 +1,137 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef PLANNING_ENVIRONMENT_COLLISION_SPACE_MONITOR_
+#define PLANNING_ENVIRONMENT_COLLISION_SPACE_MONITOR_
+
+#include "planning_environment/collision_models.h"
+#include "planning_environment/kinematic_model_state_monitor.h"
+
+#include <tf/transform_listener.h>
+#include <robot_msgs/CollisionMap.h>
+#include <robot_msgs/AttachedObject.h>
+
+namespace planning_environment
+{
+
+ /** @b CollisionSpaceMonitor is a class which in addition to being aware of a robot model,
+ is also aware of a collision space.
+
+ <hr>
+
+ @section topic ROS topics
+
+ Subscribes to (name/type):
+ - @b "collision_map"/CollisionMap : data describing the 3D environment
+ - @b "attach_object"/AttachedObject : data describing an object to be attached to a link
+ */
+ class CollisionSpaceMonitor : public KinematicModelStateMonitor
+ {
+ public:
+
+ CollisionSpaceMonitor(CollisionModels *cm) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), true),
+ tf_(*ros::Node::instance())
+ {
+ cm_ = cm;
+ setupCSM();
+ }
+
+ virtual ~CollisionSpaceMonitor(void)
+ {
+ }
+
+ collision_space::EnvironmentModel* getEnvironmentModel(void) const
+ {
+ return collisionSpace_;
+ }
+
+ CollisionModels* getCollisionModels(void) const
+ {
+ return cm_;
+ }
+
+ /** Define a callback for before updating a map */
+ void setOnBeforeMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
+ {
+ onBeforeMapUpdate_ = callback;
+ }
+
+ /** Define a callback for after updating a map */
+ void setOnAfterMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
+ {
+ onAfterMapUpdate_ = callback;
+ }
+
+ /** Define a callback for after updating a map */
+ void setOnAfterAttachBodyCallback(const boost::function<void(planning_models::KinematicModel::Link*)> &callback)
+ {
+ onAfterAttachBody_ = callback;
+ }
+
+ bool haveMap(void) const
+ {
+ return haveMap_;
+ }
+
+ /** Return true if a map update has been received in the last sec seconds */
+ bool isMapUpdated(double sec) const;
+
+ protected:
+
+ void setupCSM(void);
+ void collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap);
+ void attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject);
+
+ tf::TransformListener tf_;
+ CollisionModels *cm_;
+ collision_space::EnvironmentModel *collisionSpace_;
+
+ bool haveMap_;
+ ros::Time lastMapUpdate_;
+ ros::Subscriber attachBodySubscriber_;
+ ros::Subscriber collisionMapSubscriber_;
+
+ boost::function<void(const robot_msgs::CollisionMapConstPtr)> onBeforeMapUpdate_;
+ boost::function<void(const robot_msgs::CollisionMapConstPtr)> onAfterMapUpdate_;
+ boost::function<void(planning_models::KinematicModel::Link*)> onAfterAttachBody_;
+
+ };
+
+
+}
+
+#endif
+
Added: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-08 05:49:11 UTC (rev 16830)
@@ -0,0 +1,159 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef PLANNING_ENVIRONMENT_KINEMATIC_MODEL_STATE_MONITOR_
+#define PLANNING_ENVIRONMENT_KINEMATIC_MODEL_STATE_MONITOR_
+
+#include "planning_environment/robot_models.h"
+#include <tf/transform_datatypes.h>
+#include <robot_msgs/MechanismState.h>
+#include <robot_msgs/PoseWithCovariance.h>
+#include <boost/bind.hpp>
+#include <vector>
+#include <string>
+
+namespace planning_environment
+{
+
+ /** @b KinematicModelStateMonitor is a class that monitors the robot state for the kinematic model defined in @b RobotModels
+
+ <hr>
+
+ @section topic ROS topics
+
+ Subscribes to (name/type):
+ - @b "mechanism_model"/MechanismModel : position for each of the robot's joints
+ - @b "localized_pose"/PoseWithCovariance : localized robot pose
+
+ */
+ class KinematicModelStateMonitor
+ {
+ public:
+
+ KinematicModelStateMonitor(RobotModels *rm, bool includePose = false)
+ {
+ rm_ = rm;
+ includePose_ = includePose;
+ setupRSM();
+ }
+
+ virtual ~KinematicModelStateMonitor(void)
+ {
+ if (robotState_)
+ delete robotState_;
+ }
+
+ /** Define a callback for when the state is changed */
+ void setOnStateUpdateCallback(const boost::function<void(void)> &callback)
+ {
+ onStateUpdate_ = callback;
+ }
+
+ planning_models::KinematicModel* getKinematicModel(void) const
+ {
+ return kmodel_;
+ }
+
+ RobotModels* getRobotModels(void) const
+ {
+ return rm_;
+ }
+
+ /** Return a pointer to the maintained robot state */
+ const planning_models::KinematicModel::StateParams* getRobotState(void) const
+ {
+ return robotState_;
+ }
+
+ /** Return true if a pose has been received */
+ bool havePose(void) const
+ {
+ return havePose_;
+ }
+
+ /** Return true if a full mechanism state has been received */
+ bool haveState(void) const
+ {
+ return haveMechanismState_;
+ }
+
+ /** Wait until a pose is received */
+ void waitForPose(void) const;
+
+ /** Wait until a full mechanism state is received */
+ void waitForState(void) const;
+
+ /** Return true if a pose has been received in the last sec seconds */
+ bool isPoseUpdated(double sec) const;
+
+ /** Return true if a full mechanism state has been received in the last sec seconds */
+ bool isStateUpdated(double sec) const;
+
+ /** Output the current state as ROS INFO */
+ void printRobotState(void) const;
+
+ protected:
+
+ void setupRSM(void);
+ void localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose);
+ void mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState);
+
+
+ RobotModels *rm_;
+ bool includePose_;
+ planning_models::KinematicModel *kmodel_;
+ std::vector<std::string> planarJoints_;
+ std::vector<std::string> floatingJoints_;
+
+ ros::NodeHandle nh_;
+ ros::Subscriber mechanismStateSubscriber_;
+ ros::Subscriber localizedPoseSubscriber_;
+
+ planning_models::KinematicModel::StateParams *robotState_;
+ tf::Pose pose_;
+ boost::function<void(void)> onStateUpdate_;
+
+ bool havePose_;
+ bool haveMechanismState_;
+ ros::Time lastStateUpdate_;
+ ros::Time lastPoseUpdate_;
+ };
+
+
+}
+
+#endif
+
Property changes on: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-08 05:49:11 UTC (rev 16830)
@@ -37,7 +37,7 @@
#ifndef PLANNING_ENVIRONMENT_ROBOT_MODELS_
#define PLANNING_ENVIRONMENT_ROBOT_MODELS_
-#include "planning_models/kinematic.h"
+#include <planning_models/kinematic.h>
#include <ros/ros.h>
#include <ros/node.h>
#include <boost/shared_ptr.hpp>
@@ -49,11 +49,7 @@
namespace planning_environment
{
- /** @htmlinclude ../../manifest.html
-
- @mainpage
-
- A class capable of loading a robot model from the parameter server */
+ /** A class capable of loading a robot model from the parameter server */
class RobotModels
{
@@ -85,6 +81,7 @@
RobotModels(const std::string &description)
{
description_ = nh_.getNode()->mapName(description);
+ loaded_models_ = false;
loadRobot();
}
@@ -129,22 +126,18 @@
return self_collision_check_groups_;
}
+ bool loadedModels(void) const
+ {
+ return loaded_models_;
+ }
+
double getSelfSeePadding(void);
double getSelfSeeScale(void);
std::vector< boost::shared_ptr<PlannerConfig> > getGroupPlannersConfig(const std::string &group);
/** Reload the robot description and recreate the model */
- virtual void reload(void)
- {
- kmodel_.reset();
- urdf_.reset();
- planning_groups_.clear();
- self_see_links_.clear();
- collision_check_links_.clear();
- self_collision_check_groups_.clear();
- loadRobot();
- }
+ virtual void reload(void);
protected:
@@ -160,6 +153,7 @@
std::string description_;
+ bool loaded_models_;
boost::shared_ptr<planning_models::KinematicModel> kmodel_;
boost::shared_ptr<robot_desc::URDF> urdf_;
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-08 05:49:11 UTC (rev 16830)
@@ -11,7 +11,9 @@
<depend package="roscpp" />
<depend package="rosconsole" />
+ <depend package="tf" />
<depend package="pr2_defs" />
+ <depend package="robot_msgs" />
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-08 05:49:11 UTC (rev 16830)
@@ -35,14 +35,12 @@
/** \author Ioan Sucan */
#include "planning_environment/collision_models.h"
-#include <ros/console.h>
-#include <sstream>
void planning_environment::CollisionModels::loadCollision(void)
{
- ode_collision_model_ = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelODE());
- if (urdf_.get() && kmodel_.get())
+ if (loadedModels())
{
+ ode_collision_model_ = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelODE());
ode_collision_model_->lock();
ode_collision_model_->addRobotModel(kmodel_, collision_check_links_, scale_, padd_);
Added: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-08 05:49:11 UTC (rev 16830)
@@ -0,0 +1,149 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include "planning_environment/collision_space_monitor.h"
+
+namespace planning_environment
+{
+
+ static inline double radiusOfBox(const robot_msgs::Point32 &point)
+ {
+ return std::max(std::max(point.x, point.y), point.z) * 1.73;
+ }
+}
+
+void planning_environment::CollisionSpaceMonitor::setupCSM(void)
+{
+ onBeforeMapUpdate_ = NULL;
+ onAfterMapUpdate_ = NULL;
+ onAfterAttachBody_ = NULL;
+ haveMap_ = false;
+ collisionSpace_ = cm_->getODECollisionModel().get();
+
+ collisionMapSubscriber_ = nh_.subscribe("collision_map", 1, &CollisionSpaceMonitor::collisionMapCallback, this);
+ attachBodySubscriber_ = nh_.subscribe("attach_object", 1, &CollisionSpaceMonitor::attachObjectCallback, this);
+}
+
+bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
+{
+ if (sec > 0 && lastMapUpdate_ < ros::Time::now() - ros::Duration(sec))
+ return false;
+ else
+ return true;
+}
+
+void planning_environment::CollisionSpaceMonitor::collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
+{
+ unsigned int n = collisionMap->get_boxes_size();
+ ROS_DEBUG("Received %u points (collision map)", n);
+
+ if (onBeforeMapUpdate_ != NULL)
+ onBeforeMapUpdate_(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 ] = 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(collisionMap->boxes[i].extents);
+ }
+
+ collisionSpace_->lock();
+ collisionSpace_->clearObstacles();
+ collisionSpace_->addPointCloud(n, data);
+ collisionSpace_->unlock();
+
+ delete[] data;
+
+ double tupd = (ros::WallTime::now() - startTime).toSec();
+ ROS_DEBUG("Updated map model in %f seconds", tupd);
+ lastMapUpdate_ = ros::Time::now();
+ haveMap_ = true;
+
+ if (onAfterMapUpdate_ != NULL)
+ onAfterMapUpdate_(collisionMap);
+}
+
+void planning_environment::CollisionSpaceMonitor::attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject)
+{
+ collisionSpace_->lock();
+ planning_models::KinematicModel::Link *link = kmodel_->getLink(attachedObject->link_name);
+
+ if (link)
+ {
+ // clear the previously attached bodies
+ for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
+ delete link->attachedBodies[i];
+ unsigned int n = attachedObject->get_objects_size();
+ link->attachedBodies.resize(n);
+
+ // create the new ones
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ link->attachedBodies[i] = new planning_models::KinematicModel::AttachedBody();
+
+ robot_msgs::PointStamped center;
+ robot_msgs::PointStamped 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;
+ 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] = 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
+ collisionSpace_->updateAttachedBodies();
+ 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'", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str());
+ collisionSpace_->unlock();
+ if (link && (onAfterAttachBody_ != NULL))
+ onAfterAttachBody_(link);
+}
Added: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-08 05:49:11 UTC (rev 16830)
@@ -0,0 +1,196 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include "planning_environment/kinematic_model_state_monitor.h"
+#include <sstream>
+
+void planning_environment::KinematicModelStateMonitor::setupRSM(void)
+{
+ pose_.setIdentity();
+ kmodel_ = NULL;
+ robotState_ = NULL;
+ onStateUpdate_ = NULL;
+ havePose_ = haveMechanismState_ = false;
+ if (rm_->loadedModels())
+ {
+ kmodel_ = rm_->getKinematicModel().get();
+ robotState_ = kmodel_->newStateParams();
+ robotState_->setInRobotFrame();
+
+ for (unsigned int i = 0 ; i < kmodel_->getModelInfo().floatingJoints.size() ; ++i)
+ floatingJoints_.push_back(kmodel_->getModelInfo().parameterName[kmodel_->getModelInfo().floatingJoints[i]]);
+ for (unsigned int i = 0 ; i < kmodel_->getModelInfo().planarJoints.size() ; ++i)
+ planarJoints_.push_back(kmodel_->getModelInfo().parameterName[kmodel_->getModelInfo().planarJoints[i]]);
+
+ mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
+ if (includePose_)
+ localizedPoseSubscriber_ = nh_.subscribe("localized_pose", 1, &KinematicModelStateMonitor::localizedPoseCallback, this);
+ }
+}
+
+void planning_environment::KinematicModelStateMonitor::mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState)
+{
+ bool change = !haveMechanismState_;
+
+ static bool first_time = true;
+
+ unsigned int n = mechanismState->get_joint_states_size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ planning_models::KinematicModel::Joint* joint = kmodel_->getJoint(mechanismState->joint_states[i].name);
+ if (joint)
+ {
+ if (joint->usedParams == 1)
+ {
+ double pos = mechanismState->joint_states[i].position;
+ bool this_changed = 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)", mechanismState->joint_states[i].name.c_str(), joint->usedParams);
+ }
+ }
+ else
+ {
+ if (first_time)
+ ROS_ERROR("Unknown joint: %s", mechanismState->joint_states[i].name.c_str());
+ }
+ }
+
+ first_time = false;
+
+ lastStateUpdate_ = ros::Time::now();
+ if (!haveMechanismState_)
+ haveMechanismState_ = robotState_->seenAll();
+
+ if (change && onStateUpdate_ != NULL)
+ onStateUpdate_();
+}
+
+void planning_environment::KinematicModelStateMonitor::localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose)
+{
+ tf::PoseMsgToTF(localizedPose->pose, pose_);
+ lastPoseUpdate_ = ros::Time::now();
+
+ bool change = !havePose_;
+ havePose_ = true;
+
+ if (!planarJoints_.empty())
+ {
+ double planar_joint[3];
+ planar_joint[0] = pose_.getOrigin().x();
+ planar_joint[1] = pose_.getOrigin().y();
+
+ double yaw, pitch, roll;
+ pose_.getBasis().getEulerZYX(yaw, pitch, roll);
+ planar_joint[2] = yaw;
+
+ for (unsigned int i = 0 ; i < planarJoints_.size() ; ++i)
+ {
+ bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoints_[i]);
+ change = change || this_changed;
+ }
+ }
+
+ if (!floatingJoints_.empty())
+ {
+ double floating_joint[7];
+ floating_joint[0] = pose_.getOrigin().x();
+ floating_joint[1] = pose_.getOrigin().y();
+ floating_joint[2] = pose_.getOrigin().z();
+ btQuaternion q = pose_.getRotation();
+ floating_joint[3] = q.x();
+ floating_joint[4] = q.y();
+ floating_joint[5] = q.z();
+ floating_joint[6] = q.w();
+
+ for (unsigned int i = 0 ; i < floatingJoints_.size() ; ++i)
+ {
+ bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoints_[i]);
+ change = change || this_changed;
+ }
+ }
+
+ if (change && onStateUpdate_ != NULL)
+ onStateUpdate_();
+}
+
+void planning_environment::KinematicModelStateMonitor::waitForState(void) const
+{
+ while (nh_.ok() && !haveMechanismState_)
+ {
+ ROS_INFO("Waiting for mechanism state ...");
+ ros::spinOnce();
+ ros::Duration().fromSec(0.05).sleep();
+ }
+ ROS_INFO("Mechanism state received!");
+}
+
+void planning_environment::KinematicModelStateMonitor::waitForPose(void) const
+{
+ while (nh_.ok() && !havePose_)
+ {
+ ROS_INFO("Waiting for robot pose ...");
+ ros::spinOnce();
+ ros::Duration().fromSec(0.05).sleep();
+ }
+ ROS_INFO("Robot pose received!");
+}
+
+bool planning_environment::KinematicModelStateMonitor::isStateUpdated(double sec) const
+{
+ if (sec > 0 && lastStateUpdate_ < ros::Time::now() - ros::Duration(sec))
+ return false;
+ else
+ return true;
+}
+
+bool planning_environment::KinematicModelStateMonitor::isPoseUpdated(double sec) const
+{
+ if (sec > 0 && lastPoseUpdate_ < ros::Time::now() - ros::Duration(sec))
+ return false;
+ else
+ return true;
+}
+
+void planning_environment::KinematicModelStateMonitor::printRobotState(void) const
+{
+ std::stringstream ss;
+ robotState_->print(ss);
+ ROS_INFO("%s", ss.str().c_str());
+}
Property changes on: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-08 05:49:11 UTC (rev 16830)
@@ -38,6 +38,17 @@
#include <ros/console.h>
#include <sstream>
+void planning_environment::RobotModels::reload(void)
+{
+ kmodel_.reset();
+ urdf_.reset();
+ planning_groups_.clear();
+ self_see_links_.clear();
+ collision_check_links_.clear();
+ self_collision_check_groups_.clear();
+ loadRobot();
+}
+
void planning_environment::RobotModels::loadRobot(void)
{
std::string content;
@@ -46,6 +57,7 @@
urdf_ = boost::shared_ptr<robot_desc::URDF>(new robot_desc::URDF());
if (urdf_->loadString(content.c_str()))
{
+ loaded_models_ = true;
getPlanningGroups(planning_groups_);
kmodel_ = boost::shared_ptr<planning_models::KinematicModel>(new planning_models::KinematicModel());
kmodel_->setVerbose(false);
@@ -54,7 +66,9 @@
// make sure the kinematic model is in its own frame
// (remove all transforms caused by planar or floating
// joints)
- kmodel_->reduceToRobotFrame();
+ if (!kmodel_->reduceToRobotFrame())
+ ROS_ERROR("Bad things happen with setting the robot(s) in thir frame. It is possible differences between frames will cause collisions when planning.");
+
kmodel_->defaultState();
getCollisionCheckLinks(collision_check_links_);
Modified: pkg/trunk/world_models/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-06-08 05:49:11 UTC (rev 16830)
@@ -4,7 +4,7 @@
rospack_add_boost_directories()
-set(CMAKE_BUILD_TYPE Release)
+set(ROS_BUILD_TYPE Debug)
rospack_add_library(collision_space src/collision_space/output.cpp
src/collision_space/bodies.cpp
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
