Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18693] pkg/trunk
|
From: <is...@us...> - 2009-07-13 21:31:28
|
Revision: 18693
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18693&view=rev
Author: isucan
Date: 2009-07-13 21:31:24 +0000 (Mon, 13 Jul 2009)
Log Message:
-----------
better organization of planning_environment, fixed the way we attach objects to the robot and began work on representing objects by their model in the collision space
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/ModelBase.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp
pkg/trunk/motion_planning/planning_research/sbpl_pm_wrapper/include/sbpl_pm_wrapper/pm_wrapper.h
pkg/trunk/sandbox/chomp_motion_planner/include/chomp_motion_planner/chomp_robot_model.h
pkg/trunk/stacks/geometry/bullet/Makefile.bullet
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.cpp
pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
pkg/trunk/util/geometric_shapes/src/load_mesh.cpp
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/
pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/robot_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/
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/include/planning_environment/monitors/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/
pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/construct_object.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/src/models/
pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/models/robot_models.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/
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
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/view_state_validity.cpp
pkg/trunk/motion_planning/planning_environment/src/util/
pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp
pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/AttachedObject.msg
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/Object.msg
Removed Paths:
-------------
pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/AttachedObject.msg
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
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/include/planning_environment/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
pkg/trunk/motion_planning/planning_environment/src/view_state_validity.cpp
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/AttachedObject.msg
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/AttachedObject.msg 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/AttachedObject.msg 2009-07-13 21:31:24 UTC (rev 18693)
@@ -1,8 +0,0 @@
-# This mesage defines what objects are attached to a link. The objects
-# are assumed to be defined in the reference frame of the link they
-# are attached to
-
-Header header
-string robot_name
-string link_name
-ObjectOnTable[] objects
Modified: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -47,7 +47,7 @@
#include <motion_planning_srvs/MotionPlan.h>
#include <ros/ros.h>
-#include <planning_environment/planning_monitor.h>
+#include <planning_environment/monitors/planning_monitor.h>
namespace move_arm
{
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-13 21:31:24 UTC (rev 18693)
@@ -39,7 +39,7 @@
#include <ros/ros.h>
#include <robot_actions/action_client.h>
-#include <planning_environment/kinematic_model_state_monitor.h>
+#include <planning_environment/monitors/kinematic_model_state_monitor.h>
#include <pr2_robot_actions/MoveArmGoal.h>
#include <pr2_robot_actions/MoveArmState.h>
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/ModelBase.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/ModelBase.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/ModelBase.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -37,7 +37,7 @@
#ifndef OMPL_PLANNING_MODEL_BASE_
#define OMPL_PLANNING_MODEL_BASE_
-#include <planning_environment/planning_monitor.h>
+#include <planning_environment/monitors/planning_monitor.h>
#include <string>
namespace ompl_planning
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -38,7 +38,7 @@
#define OMPL_PLANNING_EXTENSIONS_GOAL_DEFINITIONS_
#include "ompl_planning/ModelBase.h"
-#include <planning_environment/kinematic_state_constraint_evaluator.h>
+#include <planning_environment/util/kinematic_state_constraint_evaluator.h>
#include <ompl/extension/kinematic/SpaceInformationKinematic.h>
namespace ompl_planning
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -39,7 +39,7 @@
#include <ompl/base/StateValidityChecker.h>
#include <collision_space/environment.h>
-#include <planning_environment/kinematic_state_constraint_evaluator.h>
+#include <planning_environment/util/kinematic_state_constraint_evaluator.h>
#include "ompl_planning/ModelBase.h"
#include "ompl_planning/extensions/SpaceInformation.h"
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -49,7 +49,7 @@
#include <ompl/base/Planner.h>
#include <ompl/extension/kinematic/PathSmootherKinematic.h>
-#include <planning_environment/robot_models.h>
+#include <planning_environment/models/robot_models.h>
#include <ros/console.h>
#include <string>
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-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-07-13 21:31:24 UTC (rev 18693)
@@ -41,7 +41,7 @@
**/
-#include <planning_environment/collision_space_monitor.h>
+#include <planning_environment/monitors/collision_space_monitor.h>
#include <visualization_msgs/Marker.h>
#include <std_msgs/Byte.h>
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-13 21:31:24 UTC (rev 18693)
@@ -5,19 +5,23 @@
set(ROS_BUILD_TYPE Debug)
-rospack_add_library(planning_environment src/robot_models.cpp
- src/collision_models.cpp
- src/kinematic_model_state_monitor.cpp
- src/collision_space_monitor.cpp
- src/kinematic_state_constraint_evaluator.cpp
- src/planning_monitor.cpp)
+rospack_add_library(planning_environment src/models/robot_models.cpp
+ src/models/collision_models.cpp
+ src/monitors/kinematic_model_state_monitor.cpp
+ src/monitors/collision_space_monitor.cpp
+ src/monitors/planning_monitor.cpp
+ src/util/kinematic_state_constraint_evaluator.cpp
+ src/util/construct_object.cpp)
rospack_add_openmp_flags(planning_environment)
rospack_link_boost(planning_environment thread)
# Utility apps
-rospack_add_executable(view_state_validity src/view_state_validity.cpp)
+rospack_add_executable(view_state_validity src/tools/view_state_validity.cpp)
target_link_libraries(view_state_validity planning_environment)
+rospack_add_executable(clear_known_objects src/tools/clear_known_objects.cpp)
+target_link_libraries(clear_known_objects planning_environment)
+
# Tests
# Create a model of the PR2
Deleted: 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-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -1,103 +0,0 @@
-/*********************************************************************
-* 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_MODELS_
-#define PLANNING_ENVIRONMENT_COLLISION_MODELS_
-
-#include "planning_environment/robot_models.h"
-#include <collision_space/environment.h>
-
-namespace planning_environment
-{
-
- /** \brief A class capable of loading a robot model from the parameter server */
-
- class CollisionModels : public RobotModels
- {
- public:
-
- CollisionModels(const std::string &description, double scale = 1.0, double padd = 0.0) : RobotModels(description), scale_(scale), padd_(padd)
- {
- loadCollision(collision_check_links_);
- }
-
- CollisionModels(const std::string &description, const std::vector<std::string> &links, double scale = 1.0, double padd = 0.0) : RobotModels(description), scale_(scale), padd_(padd)
- {
- loadCollision(links);
- }
-
- virtual ~CollisionModels(void)
- {
- }
-
- /** \brief Reload the robot description and recreate the model */
- virtual void reload(void)
- {
- RobotModels::reload();
- ode_collision_model_.reset();
- bullet_collision_model_.reset();
- loadCollision(collision_check_links_);
- }
-
- /** \brief Return the instance of the constructed ODE collision model */
- const boost::shared_ptr<collision_space::EnvironmentModel> &getODECollisionModel(void) const
- {
- return ode_collision_model_;
- }
-
- /** \brief Return the instance of the constructed Bullet collision model */
- const boost::shared_ptr<collision_space::EnvironmentModel> &getBulletCollisionModel(void) const
- {
- return bullet_collision_model_;
- }
-
- protected:
-
- void loadCollision(const std::vector<std::string> &links);
- void setupModel(boost::shared_ptr<collision_space::EnvironmentModel> &model, const std::vector<std::string> &links);
-
- boost::shared_ptr<collision_space::EnvironmentModel> ode_collision_model_;
- boost::shared_ptr<collision_space::EnvironmentModel> bullet_collision_model_;
-
- double scale_;
- double padd_;
- };
-
-
-}
-
-#endif
-
Deleted: 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 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -1,167 +0,0 @@
-/*********************************************************************
-* 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/message_notifier.h>
-#include <mapping_msgs/CollisionMap.h>
-#include <tabletop_msgs/AttachedObject.h>
-
-#include <boost/thread/mutex.hpp>
-
-namespace planning_environment
-{
-
- /** \brief @b CollisionSpaceMonitor is a class which in addition to being aware of a robot model,
- is also aware of a collision space.
- */
- class CollisionSpaceMonitor : public KinematicModelStateMonitor
- {
- public:
-
- CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf, frame_id)
- {
- cm_ = cm;
- setupCSM();
- }
-
- CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf)
- {
- cm_ = cm;
- setupCSM();
- }
-
- virtual ~CollisionSpaceMonitor(void)
- {
- if (collisionMapNotifier_)
- delete collisionMapNotifier_;
- if (collisionMapUpdateNotifier_)
- delete collisionMapUpdateNotifier_;
- if (attachedBodyNotifier_)
- delete attachedBodyNotifier_;
- }
-
- /** \brief Return the instance of the environment model maintained */
- collision_space::EnvironmentModel* getEnvironmentModel(void) const
- {
- return collisionSpace_;
- }
-
- /** \brief Return the instance of collision models that is being used */
- CollisionModels* getCollisionModels(void) const
- {
- return cm_;
- }
-
- /** \brief Return the scaling employed when creating spheres
- from boxes in a collision map. The radius of a sphere is
- this scaling multiplied by the largest extent of the box */
- double getBoxScale(void) const
- {
- return boxScale_;
- }
-
- /** \brief Define a callback for before updating a map */
- void setOnBeforeMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
- {
- onBeforeMapUpdate_ = callback;
- }
-
- /** \brief Define a callback for after updating a map */
- void setOnAfterMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
- {
- 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
- {
- return haveMap_;
- }
-
- /** \brief Return true if a map update has been received in the last sec seconds. If sec < 10us, this function always returns true. */
- bool isMapUpdated(double sec) const;
-
-
- /** \brief Wait until a map is received */
- void waitForMap(void) const;
-
- /** \brief Return the last update time for the map */
- const ros::Time& lastMapUpdate(void) const
- {
- return lastMapUpdate_;
- }
-
- protected:
-
- void setupCSM(void);
- 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 tabletop_msgs::AttachedObjectConstPtr &attachedObject);
-
- CollisionModels *cm_;
- collision_space::EnvironmentModel *collisionSpace_;
- double boxScale_;
- boost::mutex mapUpdateLock_;
-
- bool haveMap_;
- ros::Time lastMapUpdate_;
- tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapNotifier_;
- tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapUpdateNotifier_;
- tf::MessageNotifier<tabletop_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_;
-
- };
-
-
-}
-
-#endif
-
Deleted: 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 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -1,196 +0,0 @@
-/*********************************************************************
-* 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 <tf/transform_listener.h>
-#include <mechanism_msgs/MechanismState.h>
-#include <boost/bind.hpp>
-#include <vector>
-#include <string>
-
-namespace planning_environment
-{
-
- /** \brief @b KinematicModelStateMonitor is a class that monitors the robot state for the kinematic model defined in @b RobotModels
- If the pose is not included, the robot state is the frame of the link it attaches to the world. If the pose is included,
- the frame of the robot is the one in which the pose is published.
- */
- class KinematicModelStateMonitor
- {
- public:
-
- KinematicModelStateMonitor(RobotModels *rm, tf::TransformListener *tf)
- {
- rm_ = rm;
- tf_ = tf;
- includePose_ = false;
- setupRSM();
- }
-
- KinematicModelStateMonitor(RobotModels *rm, tf::TransformListener *tf, const std::string &frame_id)
- {
- rm_ = rm;
- tf_ = tf;
- includePose_ = true;
- frame_id_ = frame_id;
- setupRSM();
- }
-
- virtual ~KinematicModelStateMonitor(void)
- {
- if (robotState_)
- delete robotState_;
- }
-
- /** \brief Define a callback for when the state is changed */
- void setOnStateUpdateCallback(const boost::function<void(void)> &callback)
- {
- onStateUpdate_ = callback;
- }
-
- /** \brief Get the kinematic model that is being monitored */
- planning_models::KinematicModel* getKinematicModel(void) const
- {
- return kmodel_;
- }
-
- /** \brief Get the instance of @b RobotModels that is being used */
- RobotModels* getRobotModels(void) const
- {
- return rm_;
- }
-
- /** \brief Return a pointer to the maintained robot state */
- const planning_models::StateParams* getRobotState(void) const
- {
- return robotState_;
- }
-
- /** \brief Return the transform listener */
- tf::TransformListener *getTransformListener(void) const
- {
- return tf_;
- }
-
- /** \brief Return the frame id of the state */
- const std::string& getFrameId(void) const
- {
- return frame_id_;
- }
-
- /** \brief Return the robot frame id (robot without pose) */
- const std::string& getRobotFrameId(void) const
- {
- return robot_frame_;
- }
-
- /** \brief Return true if a full mechanism state has been received (including pose, if pose inclusion is enabled) */
- bool haveState(void) const
- {
- return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
- }
-
- /** \brief Return the time of the last state update */
- const ros::Time& lastMechanismStateUpdate(void) const
- {
- return lastMechanismStateUpdate_;
- }
-
- /** \brief Return the time of the last pose update */
- const ros::Time& lastPoseUpdate(void) const
- {
- return lastPoseUpdate_;
- }
-
- /** \brief Wait until a full mechanism state is received (including pose, if pose inclusion is enabled) */
- void waitForState(void) const;
-
- /** \brief Return true if a mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
- bool isMechanismStateUpdated(double sec) const;
-
- /** \brief Return true if a pose has been received in the last
- sec seconds. If sec < 10us, this function always returns
- true. */
- bool isPoseUpdated(double sec) const;
-
- /** \brief Return true if the pose is included in the state */
- bool isPoseIncluded(void) const
- {
- return includePose_;
- }
-
- /** \brief Output the current state as ROS INFO */
- void printRobotState(void) const;
-
- protected:
-
- void setupRSM(void);
- void mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState);
-
-
- RobotModels *rm_;
- bool includePose_;
- planning_models::KinematicModel *kmodel_;
- std::string planarJoint_;
- std::string floatingJoint_;
-
- ros::NodeHandle nh_;
- ros::Subscriber mechanismStateSubscriber_;
- tf::TransformListener *tf_;
-
- /** \brief How long to wait for a TF if it is not yet available, before failing */
- ros::Duration tfWait_;
-
- planning_models::StateParams *robotState_;
- tf::Pose pose_;
- std::string robot_frame_;
- std::string frame_id_;
-
- boost::function<void(void)> onStateUpdate_;
-
- bool havePose_;
- bool haveMechanismState_;
- ros::Time lastMechanismStateUpdate_;
- ros::Time lastPoseUpdate_;
- };
-
-}
-
-#endif
Deleted: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -1,203 +0,0 @@
-/*********************************************************************
-* 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_STATE_CONSTRAINT_EVALUATOR_
-#define PLANNING_ENVIRONMENT_KINEMATIC_STATE_CONSTRAINT_EVALUATOR_
-
-#include <planning_models/kinematic.h>
-#include <motion_planning_msgs/KinematicConstraints.h>
-#include <angles/angles.h>
-#include <boost/shared_ptr.hpp>
-#include <iostream>
-#include <vector>
-
-namespace planning_environment
-{
-
- class KinematicConstraintEvaluator
- {
- public:
-
- KinematicConstraintEvaluator(void)
- {
- }
-
- virtual ~KinematicConstraintEvaluator(void)
- {
- }
-
- /** \brief Clear the stored constraint */
- virtual void clear(void) = 0;
-
- /** \brief This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
- virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc) = 0;
-
- /** \brief Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
- virtual bool decide(const double *params, const int groupID) const = 0;
-
- /** \brief Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
- virtual bool decide(const double *params) const;
-
- /** \brief Print the constraint data */
- virtual void print(std::ostream &out = std::cout) const
- {
- }
- };
-
- class JointConstraintEvaluator : public KinematicConstraintEvaluator
- {
- public:
-
- JointConstraintEvaluator(void) : KinematicConstraintEvaluator()
- {
- m_joint = NULL;
- m_kmodel = NULL;
- }
-
- /** \brief This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
- virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc);
-
- /** \brief This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
- bool use(const planning_models::KinematicModel *kmodel, const motion_planning_msgs::JointConstraint &jc);
-
- /** \brief Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
- virtual bool decide(const double *params, const int groupID) const;
-
- /** \brief Clear the stored constraint */
- virtual void clear(void);
-
- /** \brief Print the constraint data */
- virtual void print(std::ostream &out = std::cout) const;
-
- /** \brief Get the constraint message */
- const motion_planning_msgs::JointConstraint& getConstraintMessage(void) const;
-
- protected:
-
- motion_planning_msgs::JointConstraint m_jc;
- planning_models::KinematicModel::Joint *m_joint;
- const planning_models::KinematicModel *m_kmodel;
-
- };
-
-
- class PoseConstraintEvaluator : public KinematicConstraintEvaluator
- {
- public:
-
- PoseConstraintEvaluator(void) : KinematicConstraintEvaluator()
- {
- m_link = NULL;
- }
-
- /** \brief This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
- virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc);
-
- /** \brief This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
- bool use(const planning_models::KinematicModel *kmodel, const motion_planning_msgs::PoseConstraint &pc);
-
- /** \brief Clear the stored constraint */
- virtual void clear(void);
-
- /** \brief Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
- virtual bool decide(const double *params, int groupID) const;
-
- /** \brief Evaluate the distances to the position and to the orientation are given. */
- void evaluate(double *distPos, double *distAng) const;
-
- /** \brief Decide whether the constraint is satisfied. The distances to the position and to the orientation are given. */
- bool decide(double dPos, double dAng) const;
-
- /** \brief Print the constraint data */
- void print(std::ostream &out = std::cout) const;
-
- /** \brief Get the constraint message */
- const motion_planning_msgs::PoseConstraint& getConstraintMessage(void) const;
-
- protected:
-
- motion_planning_msgs::PoseConstraint m_pc;
- double m_x;
- double m_y;
- double m_z;
- double m_yaw;
- double m_pitch;
- double m_roll;
- planning_models::KinematicModel::Link *m_link;
-
- };
-
-
- class KinematicConstraintEvaluatorSet
- {
- public:
-
- KinematicConstraintEvaluatorSet(void)
- {
- }
-
- ~KinematicConstraintEvaluatorSet(void)
- {
- clear();
- }
-
- /** \brief Clear the stored constraints */
- void clear(void);
-
- /** \brief Add a set of joint constraints */
- bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::JointConstraint> &jc);
-
- /** \brief Add a set of pose constraints */
- bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &kc);
-
- /** \brief Decide whether the set of constraints is satisfied */
- bool decide(const double *params, int groupID) const;
-
- /** \brief Decide whether the set of constraints is satisfied */
- bool decide(const double *params) const;
-
- /** \brief Print the constraint data */
- void print(std::ostream &out = std::cout) const;
-
- protected:
-
- std::vector<KinematicConstraintEvaluator*> m_kce;
-
- };
-} // planning_environment
-
-
-#endif
Copied: pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h (from rev 18686, pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -0,0 +1,103 @@
+/*********************************************************************
+* 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_MODELS_COLLISION_MODELS_
+#define PLANNING_ENVIRONMENT_MODELS_COLLISION_MODELS_
+
+#include "planning_environment/models/robot_models.h"
+#include <collision_space/environment.h>
+
+namespace planning_environment
+{
+
+ /** \brief A class capable of loading a robot model from the parameter server */
+
+ class CollisionModels : public RobotModels
+ {
+ public:
+
+ CollisionModels(const std::string &description, double scale = 1.0, double padd = 0.0) : RobotModels(description), scale_(scale), padd_(padd)
+ {
+ loadCollision(collision_check_links_);
+ }
+
+ CollisionModels(const std::string &description, const std::vector<std::string> &links, double scale = 1.0, double padd = 0.0) : RobotModels(description), scale_(scale), padd_(padd)
+ {
+ loadCollision(links);
+ }
+
+ virtual ~CollisionModels(void)
+ {
+ }
+
+ /** \brief Reload the robot description and recreate the model */
+ virtual void reload(void)
+ {
+ RobotModels::reload();
+ ode_collision_model_.reset();
+ bullet_collision_model_.reset();
+ loadCollision(collision_check_links_);
+ }
+
+ /** \brief Return the instance of the constructed ODE collision model */
+ const boost::shared_ptr<collision_space::EnvironmentModel> &getODECollisionModel(void) const
+ {
+ return ode_collision_model_;
+ }
+
+ /** \brief Return the instance of the constructed Bullet collision model */
+ const boost::shared_ptr<collision_space::EnvironmentModel> &getBulletCollisionModel(void) const
+ {
+ return bullet_collision_model_;
+ }
+
+ protected:
+
+ void loadCollision(const std::vector<std::string> &links);
+ void setupModel(boost::shared_ptr<collision_space::EnvironmentModel> &model, const std::vector<std::string> &links);
+
+ boost::shared_ptr<collision_space::EnvironmentModel> ode_collision_model_;
+ boost::shared_ptr<collision_space::EnvironmentModel> bullet_collision_model_;
+
+ double scale_;
+ double padd_;
+ };
+
+
+}
+
+#endif
+
Property changes on: pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/motion_planning/planning_environment/include/planning_environment/collision_models.h:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/robot_models.h (from rev 18686, pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/robot_models.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/robot_models.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -0,0 +1,187 @@
+/*********************************************************************
+* 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_MODELS_ROBOT_MODELS_
+#define PLANNING_ENVIRONMENT_MODELS_ROBOT_MODELS_
+
+#include <planning_models/kinematic.h>
+#include <planning_models/kinematic_state_params.h>
+#include <ros/ros.h>
+#include <ros/node.h>
+#include <boost/shared_ptr.hpp>
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace planning_environment
+{
+
+ /** \brief A class capable of loading a robot model from the parameter server */
+
+ class RobotModels
+ {
+ public:
+
+ /** \brief A class to define a planner configuration */
+ class PlannerConfig
+ {
+ public:
+ PlannerConfig(const std::string &description, const std::string &config) : description_(description), config_(config)
+ {
+ }
+
+ ~PlannerConfig(void)
+ {
+ }
+
+ const std::string& getName(void);
+ bool hasParam(const std::string ¶m);
+ std::string getParamString(const std::string ¶m, const std::string &def = "");
+ double getParamDouble(const std::string ¶m, double def);
+
+ private:
+
+ std::string description_;
+ std::string config_;
+ ros::NodeHandle nh_;
+ };
+
+
+ RobotModels(const std::string &description)
+ {
+ description_ = nh_.resolveName(description);
+ loaded_models_ = false;
+ loadRobot();
+ }
+
+ virtual ~RobotModels(void)
+ {
+ }
+
+ /** \brief Return the name of the description */
+ const std::string &getDescription(void) const
+ {
+ return description_;
+ }
+
+ /** \brief Return the instance of the constructed kinematic model */
+ const boost::shared_ptr<planning_models::KinematicModel> &getKinematicModel(void) const
+ {
+ return kmodel_;
+ }
+
+ /** \brief Return the instance of the parsed robot description */
+ const boost::shared_ptr<robot_desc::URDF> &getParsedDescription(void) const
+ {
+ return urdf_;
+ }
+
+ /** \brief Return the map of the planning groups (arrays of link names) */
+ const std::map< std::string, std::vector<std::string> > &getPlanningGroups(void) const
+ {
+ return planning_groups_;
+ }
+
+ /** \brief Return the names of the links that should be considered when performing collision checking */
+ const std::vector<std::string> &getCollisionCheckLinks(void) const
+ {
+ return collision_check_links_;
+ }
+
+ /** \brief Return the names of the links that should be considered when cleaning sensor data
+ of parts the robot can see from itself */
+ const std::vector<std::string> &getSelfSeeLinks(void) const
+ {
+ return self_see_links_;
+ }
+
+ /** \brief Return the groups of links that should be considered when testing for self collision. This is an
+ array of pairs. Both elements of the pair are groups of links. If any link in the first member of the pair
+ collides with some link in the second member of the pair, we have a collision */
+ const std::vector< std::pair < std::vector<std::string>, std::vector<std::string> > > &getSelfCollisionGroups(void) const
+ {
+ return self_collision_check_groups_;
+ }
+
+ /** \brief Return true if models have been loaded */
+ bool loadedModels(void) const
+ {
+ return loaded_models_;
+ }
+
+ /** \brief Get the amount of padding to be used for links when cleaning sensor data */
+ double getSelfSeePadding(void);
+
+ /** \brief Get the amount of scaling to be used for links when cleaning sensor data */
+ double getSelfSeeScale(void);
+
+ /** \breif Get the list of planner configurations available for a specific planning group */
+ std::vector< boost::shared_ptr<PlannerConfig> > getGroupPlannersConfig(const std::string &group);
+
+ /** \brief Reload the robot description and recreate the model */
+ virtual void reload(void);
+
+ protected:
+
+ void loadRobot(void);
+ void getPlanningGroups(std::map< std::string, std::vector<std::string> > &groups);
+
+ void getSelfSeeLinks(std::vector<std::string> &links);
+ void getCollisionCheckLinks(std::vector<std::string> &links);
+ void getSelfCollisionGroups(std::vector< std::pair < std::vector<std::string>, std::vector<std::string> > > &groups);
+
+
+ ros::NodeHandle nh_;
+
+ std::string description_;
+
+ bool loaded_models_;
+ boost::shared_ptr<planning_models::KinematicModel> kmodel_;
+ boost::shared_ptr<robot_desc::URDF> urdf_;
+
+ std::map< std::string, std::vector<std::string> > planning_groups_;
+ std::vector<std::string> self_see_links_;
+ std::vector<std::string> collision_check_links_;
+ std::vector< std::pair < std::vector<std::string>, std::vector<std::string> > >
+ self_collision_check_groups_;
+
+ };
+
+}
+
+#endif
+
Copied: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h (from rev 18686, pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -0,0 +1,167 @@
+/*********************************************************************
+* 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_MONITORS_COLLISION_SPACE_MONITOR_
+#define PLANNING_ENVIRONMENT_MONITORS_COLLISION_SPACE_MONITOR_
+
+#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>
+
+namespace planning_environment
+{
+
+ /** \brief @b CollisionSpaceMonitor is a class which in addition to being aware of a robot model,
+ is also aware of a collision space.
+ */
+ class CollisionSpaceMonitor : public KinematicModelStateMonitor
+ {
+ public:
+
+ CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf, frame_id)
+ {
+ cm_ = cm;
+ setupCSM();
+ }
+
+ CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf)
+ {
+ cm_ = cm;
+ setupCSM();
+ }
+
+ virtual ~CollisionSpaceMonitor(void)
+ {
+ if (collisionMapNotifier_)
+ delete collisionMapNotifier_;
+ if (collisionMapUpdateNotifier_)
+ delete collisionMapUpdateNotifier_;
+ if (attachedBodyNotifier_)
+ delete attachedBodyNotifier_;
+ }
+
+ /** \brief Return the instance of the environment model maintained */
+ collision_space::EnvironmentModel* getEnvironmentModel(void) const
+ {
+ return collisionSpace_;
+ }
+
+ /** \brief Return the instance of collision models that is being used */
+ CollisionModels* getCollisionModels(void) const
+ {
+ return cm_;
+ }
+
+ /** \brief Return the scaling employed when creating spheres
+ from boxes in a collision map. The radius of a sphere is
+ this scaling multiplied by the largest extent of the box */
+ double getBoxScale(void) const
+ {
+ return boxScale_;
+ }
+
+ /** \brief Define a callback for before updating a map */
+ void setOnBeforeMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
+ {
+ onBeforeMapUpdate_ = callback;
+ }
+
+ /** \brief Define a callback for after updating a map */
+ void setOnAfterMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
+ {
+ 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
+ {
+ return haveMap_;
+ }
+
+ /** \brief Return true if a map update has been received in the last sec seconds. If sec < 10us, this function always returns true. */
+ bool isMapUpdated(double sec) const;
+
+
+ /** \brief Wait until a map is received */
+ void waitForMap(void) const;
+
+ /** \brief Return the last update time for the map */
+ const ros::Time& lastMapUpdate(void) const
+ {
+ return lastMapUpdate_;
+ }
+
+ protected:
+
+ void setupCSM(void);
+ 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);
+
+ CollisionModels *cm_;
+ collision_space::EnvironmentModel *collisionSpace_;
+ double boxScale_;
+ boost::mutex mapUpdateLock_;
+
+ bool haveMap_;
+ 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_;
+
+ };
+
+
+}
+
+#endif
+
Property changes on: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h (from rev 18686, pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -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.
+********************************************************...
[truncated message content] |
