Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16750] pkg/trunk
|
From: <is...@us...> - 2009-06-05 01:17:35
|
Revision: 16750
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16750&view=rev
Author: isucan
Date: 2009-06-05 01:17:32 +0000 (Fri, 05 Jun 2009)
Log Message:
-----------
using yaml to load parameters
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/ompl_planning/manifest.xml
pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/CMakeLists.txt
pkg/trunk/motion_planning/plan_ompl_path/manifest.xml
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
pkg/trunk/util/self_watch/manifest.xml
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/visualization/rviz/manifest.xml
pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.h
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/motion_planning/planning_environment/Makefile
pkg/trunk/motion_planning/planning_environment/collision_checks.yaml
pkg/trunk/motion_planning/planning_environment/include/
pkg/trunk/motion_planning/planning_environment/include/planning_environment/
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/mainpage.dox
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/planning.yaml
pkg/trunk/motion_planning/planning_environment/src/
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
pkg/trunk/motion_planning/planning_environment/test/
pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp
pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml
Removed Paths:
-------------
pkg/trunk/motion_planning/ompl_planning/collision_checks.yaml
pkg/trunk/motion_planning/ompl_planning/planning.yaml
Deleted: pkg/trunk/motion_planning/ompl_planning/collision_checks.yaml
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/collision_checks.yaml 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/ompl_planning/collision_checks.yaml 2009-06-05 01:17:32 UTC (rev 16750)
@@ -1,76 +0,0 @@
-## This file should be loaded under /collision
-
-
-## links for which collision checking with the environment should be performed
-collision_links:
- base_link
- torso_lift_link
- head_pan_link
- head_tilt_link
- r_shoulder_pan_link
- r_shoulder_lift_link
- r_upper_arm_roll_link
- r_upper_arm_link
- r_elbow_flex_link
- r_forearm_roll_link
- r_forearm_link
- r_wrist_flex_link
- r_wrist_roll_link
- r_gripper_palm_link
- r_gripper_l_finger_link
- r_gripper_r_finger_link
- r_gripper_l_finger_tip_link
- r_gripper_r_finger_tip_link
-
-## self collision is performed between groups of links, usually pairs
-self_collision_groups: scg1 scg2 scg3 scg4 scg5 scg6
-
-scg1:
- r_forearm_link
- base_link
-
-scg2:
- r_gripper_palm_link
- base_link
-
-scg3:
- r_gripper_l_finger_link
- base_link
-
-scg4:
- r_gripper_r_finger_link
- base_link
-
-scg5:
- r_gripper_l_finger_tip_link
- base_link
-
-scg6:
- r_gripper_r_finger_tip_link
- base_link
-
-
-## list of links that the robot can see with its sensors (used to remove
-## parts of the robot from scanned data)
-self_see:
- l_upper_arm_roll_link
- r_upper_arm_roll_link
- l_elbow_flex_link
- r_elbow_flex_link
- l_forearm_roll_link
- r_forearm_roll_link
- l_wrist_flex_link
- r_wrist_flex_link
- l_wrist_roll_link
- r_wrist_roll_link
- l_gripper_l_finger_link
- l_gripper_r_finger_link
- r_gripper_l_finger_link
- r_gripper_r_finger_link
- r_shoulder_pan_link
- r_shoulder_lift_link
- l_shoulder_pan_link
- l_shoulder_lift_link
- torso_lift_link
- base_link
-
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-06-05 01:17:32 UTC (rev 16750)
@@ -122,7 +122,6 @@
bool setCollisionState(motion_planning_srvs::CollisionCheckState::Request &req, motion_planning_srvs::CollisionCheckState::Response &res);
virtual void loadRobotDescription(void);
- virtual void defaultPosition(void);
bool isMapUpdated(double sec);
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-06-05 01:17:32 UTC (rev 16750)
@@ -44,7 +44,7 @@
#include <boost/shared_ptr.hpp>
#include <urdf/URDF.h>
-#include <planning_models/kinematic.h>
+#include <planning_environment/collision_models.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <sstream>
@@ -115,7 +115,6 @@
void setIncludeBaseInState(bool value);
virtual void loadRobotDescription(void);
- virtual void defaultPosition(void);
bool loadedRobot(void) const;
void waitForState(void);
@@ -141,6 +140,9 @@
tf::TransformListener m_tf;
+ boost::shared_ptr<planning_environment::CollisionModels>
+ m_envModels;
+
boost::shared_ptr<robot_desc::URDF> m_urdf;
boost::shared_ptr<planning_models::KinematicModel> m_kmodel;
Modified: pkg/trunk/motion_planning/ompl_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/manifest.xml 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/ompl_planning/manifest.xml 2009-06-05 01:17:32 UTC (rev 16750)
@@ -17,10 +17,7 @@
<depend package="motion_planning_msgs" />
<depend package="motion_planning_srvs" />
- <depend package="wg_robot_description_parser"/>
- <depend package="planning_models"/>
- <depend package="collision_space"/>
-
+ <depend package="planning_environment"/>
<depend package="ompl" />
<export>
Modified: pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-05 01:17:32 UTC (rev 16750)
@@ -1,7 +1,7 @@
<launch>
- <rosparam command="load" ns="planning" file="$(find ompl_planning)/planning.yaml" />
- <rosparam command="load" ns="collision" file="$(find ompl_planning)/collision_checks.yaml" />
+ <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find planning_environment)/planning.yaml" />
+ <rosparam command="load" ns="robotdesc/pr2_collision" file="$(find planning_environment)/collision_checks.yaml" />
<node pkg="ompl_planning" type="motion_planner" args="robot_description:=robotdesc/pr2 collision_map:=collision_map_buffer" respawn="true" output="screen">
<param name="refresh_interval_collision_map" type="double" value="0.0" />
<param name="refresh_interval_kinematic_state" type="double" value="0.0" />
Deleted: pkg/trunk/motion_planning/ompl_planning/planning.yaml
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/planning.yaml 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/ompl_planning/planning.yaml 2009-06-05 01:17:32 UTC (rev 16750)
@@ -1,74 +0,0 @@
-## This file should be loaded under /planning
-
-## the list of groups for which motion planning can be performed
-group_list:
- base
- left_arm
- right_arm
- arms
- base_left_arm
- base_right_arm
- base_arms
-
-## the definition of each group
-groups:
-
- base:
- links:
- base_link
- planner_configs:
- RRTConfig1 SBLConfig1
-
- left_arm:
- links:
- l_shoulder_pan_link
- l_shoulder_lift_link
- l_upper_arm_roll_link
- l_upper_arm_link
- l_elbow_flex_link
- l_forearm_roll_link
- l_forearm_link
- l_wrist_flex_link
- l_wrist_roll_link
- planner_configs:
- RRTConfig2 SBLConfig2
-
- right_arm:
- links:
- r_shoulder_pan_link
- r_shoulder_lift_link
- r_upper_arm_roll_link
- r_upper_arm_link
- r_elbow_flex_link
- r_forearm_roll_link
- r_forearm_link
- r_wrist_flex_link
- r_wrist_roll_link
- planner_configs:
- RRTConfig2 SBLConfig2
-
-## the planner configurations; each config must have a type, which specifies
-## the planner to be used; other parameters can be specified as well
-
-planner_configs:
-
- RRTConfig1:
- type: RRT
- range: 0.75
-
- SBLConfig1:
- type: SBL
- projection: 0 1
- celldim: 1 1
- range: 0.5
-
- RRTConfig2:
- type: RRT
- range: 0.75
-
- SBLConfig2:
- type: SBL
- projection: 5 6
- celldim: 0.1 0.1
-
-
\ No newline at end of file
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp 2009-06-05 01:17:32 UTC (rev 16750)
@@ -119,31 +119,9 @@
{
KinematicStateMonitor::loadRobotDescription();
if (m_kmodel)
- {
- std::vector<std::string> links;
- robot_desc::URDF::Group *g = m_urdf->getGroup("collision_check");
- if (g && g->hasFlag("collision"))
- links = g->linkNames;
-
- m_collisionSpace->lock();
- unsigned int cid = m_collisionSpace->addRobotModel(m_kmodel, links);
- std::vector<robot_desc::URDF::Group*> groups;
- m_urdf->getGroups(groups);
-
- for (unsigned int i = 0 ; i < groups.size() ; ++i)
- if (groups[i]->hasFlag("self_collision"))
- m_collisionSpace->addSelfCollisionGroup(cid, groups[i]->linkNames);
- m_collisionSpace->unlock();
- }
+ m_collisionSpace = m_envModels->getODECollisionModel();
}
-void kinematic_planning::CollisionSpaceMonitor::defaultPosition(void)
-{
- KinematicStateMonitor::defaultPosition();
- if (m_collisionSpace && m_collisionSpace->getModelCount() == 1)
- m_collisionSpace->updateRobotModel(0);
-}
-
bool kinematic_planning::CollisionSpaceMonitor::isMapUpdated(double sec)
{
if (sec > 0 && m_lastMapUpdate < ros::Time::now() - ros::Duration(sec))
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-05 01:17:32 UTC (rev 16750)
@@ -35,6 +35,7 @@
/** \author Ioan Sucan */
#include "kinematic_planning/KinematicStateMonitor.h"
+#include <sstream>
void kinematic_planning::KinematicStateMonitor::kinematicStateSubscribe(void)
{
@@ -59,36 +60,16 @@
void kinematic_planning::KinematicStateMonitor::loadRobotDescription(void)
{
- std::string content;
- if (m_nodeHandle.getParam("robot_description", content))
+ if (m_nodeHandle.hasParam("robot_description"))
{
- m_urdf = boost::shared_ptr<robot_desc::URDF>(new robot_desc::URDF());
- if (m_urdf->loadString(content.c_str()))
- {
- m_kmodel = boost::shared_ptr<planning_models::KinematicModel>(new planning_models::KinematicModel());
- m_kmodel->setVerbose(false);
- m_kmodel->build(*m_urdf);
- m_kmodel->reduceToRobotFrame();
-
- m_robotState = boost::shared_ptr<planning_models::KinematicModel::StateParams>(m_kmodel->newStateParams());
- m_robotState->setInRobotFrame();
-
- m_haveMechanismState = false;
- m_haveBasePos = false;
- }
- else
- m_urdf.reset();
+ m_envModels = boost::shared_ptr<planning_environment::CollisionModels>(new planning_environment::CollisionModels("robot_description"));
+ m_urdf = m_envModels->getParsedDescription();
+ m_kmodel = m_envModels->getKinematicModel();
}
else
ROS_ERROR("Robot model not found! Did you remap robot_description?");
}
-void kinematic_planning::KinematicStateMonitor::defaultPosition(void)
-{
- if (m_kmodel)
- m_kmodel->defaultState();
-}
-
bool kinematic_planning::KinematicStateMonitor::loadedRobot(void) const
{
return m_kmodel;
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-05 01:17:32 UTC (rev 16750)
@@ -490,7 +490,6 @@
virtual void loadRobotDescription(void)
{
CollisionSpaceMonitor::loadRobotDescription();
- defaultPosition();
ROS_DEBUG("=======================================");
std::stringstream ss;
Modified: pkg/trunk/motion_planning/plan_ompl_path/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/CMakeLists.txt 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/plan_ompl_path/CMakeLists.txt 2009-06-05 01:17:32 UTC (rev 16750)
@@ -2,6 +2,8 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(plan_ompl_path)
+set(ROS_BUILD_TYPE Debug)
+
rospack_add_executable(plan_kinematic_path src/plan_kinematic_path.cpp)
rospack_add_executable(rarm_execute_plan_to_state src/right_arm/execute_plan_to_state.cpp)
Modified: pkg/trunk/motion_planning/plan_ompl_path/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/manifest.xml 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/plan_ompl_path/manifest.xml 2009-06-05 01:17:32 UTC (rev 16750)
@@ -18,7 +18,7 @@
<depend package="pr2_msgs" />
<depend package="pr2_srvs" />
- <depend package="planning_models" />
+ <depend package="planning_environment" />
<depend package="ompl_planning" />
<depend package="robot_mechanism_controllers" />
Added: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+rospack(planning_environment)
+
+rospack_add_library(planning_environment src/robot_models.cpp
+ src/collision_models.cpp)
+
+
+# Tests
+
+# Create a model of the PR2
+rospack_add_executable(test_robot_models test/test_robot_models.cpp)
+rospack_declare_test(test_robot_models)
+rospack_add_gtest_build_flags(test_robot_models)
+target_link_libraries(test_robot_models planning_environment)
+rospack_add_rostest(test/test_robot_models.xml)
Added: pkg/trunk/motion_planning/planning_environment/Makefile
===================================================================
--- pkg/trunk/motion_planning/planning_environment/Makefile (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/Makefile 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Copied: pkg/trunk/motion_planning/planning_environment/collision_checks.yaml (from rev 16707, pkg/trunk/motion_planning/ompl_planning/collision_checks.yaml)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/collision_checks.yaml (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/collision_checks.yaml 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,76 @@
+## This file should be loaded under <robot description>_collision
+
+
+## links for which collision checking with the environment should be performed
+collision_links:
+ base_link
+ torso_lift_link
+ head_pan_link
+ head_tilt_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ r_upper_arm_roll_link
+ r_upper_arm_link
+ r_elbow_flex_link
+ r_forearm_roll_link
+ r_forearm_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_palm_link
+ r_gripper_l_finger_link
+ r_gripper_r_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_tip_link
+
+## self collision is performed between groups of links, usually pairs
+self_collision_groups: scg1 scg2 scg3 scg4 scg5 scg6
+
+scg1:
+ r_forearm_link
+ base_link
+
+scg2:
+ r_gripper_palm_link
+ base_link
+
+scg3:
+ r_gripper_l_finger_link
+ base_link
+
+scg4:
+ r_gripper_r_finger_link
+ base_link
+
+scg5:
+ r_gripper_l_finger_tip_link
+ base_link
+
+scg6:
+ r_gripper_r_finger_tip_link
+ base_link
+
+
+## list of links that the robot can see with its sensors (used to remove
+## parts of the robot from scanned data)
+self_see:
+ l_upper_arm_roll_link
+ r_upper_arm_roll_link
+ l_elbow_flex_link
+ r_elbow_flex_link
+ l_forearm_roll_link
+ r_forearm_roll_link
+ l_wrist_flex_link
+ r_wrist_flex_link
+ l_wrist_roll_link
+ r_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_r_finger_link
+ r_gripper_l_finger_link
+ r_gripper_r_finger_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ torso_lift_link
+ base_link
+
Property changes on: pkg/trunk/motion_planning/planning_environment/collision_checks.yaml
___________________________________________________________________
Added: svn:mergeinfo
+
Added: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,120 @@
+/*********************************************************************
+* 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/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 */
+
+ 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();
+ }
+
+ virtual ~CollisionModels(void)
+ {
+ }
+
+ /** Reload the robot description and recreate the model */
+ virtual void reload(void)
+ {
+ RobotModels::reload();
+ ode_collision_model_.reset();
+ self_see_links_.clear();
+ collision_check_links_.clear();
+ self_collision_check_groups_.clear();
+ loadCollision();
+ }
+
+ /** Return the instance of the constructed ODE collision model */
+ const boost::shared_ptr<collision_space::EnvironmentModel> &getODECollisionModel(void) const
+ {
+ return ode_collision_model_;
+ }
+
+ const std::vector<std::string> &getCollisionCheckLinks(void) const
+ {
+ return collision_check_links_;
+ }
+
+ const std::vector<std::string> &getSelfSeeLinks(void) const
+ {
+ return self_see_links_;
+ }
+
+ const std::vector< std::vector<std::string> > &getSelfCollisionGroups(void) const
+ {
+ return self_collision_check_groups_;
+ }
+
+ protected:
+
+ void loadCollision(void);
+ void getSelfSeeLinks(std::vector<std::string> &links);
+ void getCollisionCheckLinks(std::vector<std::string> &links);
+ void getSelfCollisionGroups(std::vector< std::vector<std::string> > &groups);
+
+ std::vector<std::string> self_see_links_;
+ std::vector<std::string> collision_check_links_;
+ std::vector< std::vector<std::string> > self_collision_check_groups_;
+
+ boost::shared_ptr<collision_space::EnvironmentModel> ode_collision_model_;
+
+ double scale_;
+ double padd_;
+ };
+
+
+}
+
+#endif
+
Added: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,145 @@
+/*********************************************************************
+* 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_ROBOT_MODELS_
+#define PLANNING_ENVIRONMENT_ROBOT_MODELS_
+
+#include "planning_models/kinematic.h"
+#include <ros/ros.h>
+#include <boost/shared_ptr.hpp>
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace planning_environment
+{
+
+ /** @htmlinclude ../../manifest.html
+
+ @mainpage
+
+ A class capable of loading a robot model from the parameter server */
+
+ class RobotModels
+ {
+ public:
+
+ /** 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)
+ {
+ }
+
+ bool hasParam(const std::string ¶m);
+ std::string getParam(const std::string ¶m);
+
+ private:
+
+ std::string description_;
+ std::string config_;
+ ros::NodeHandle nh_;
+ };
+
+
+ RobotModels(const std::string &description) : description_(description)
+ {
+ loadRobot();
+ }
+
+ virtual ~RobotModels(void)
+ {
+ }
+
+ const std::string &getDescription(void) const
+ {
+ return description_;
+ }
+
+ /** Return the instance of the constructed kinematic model */
+ const boost::shared_ptr<planning_models::KinematicModel> &getKinematicModel(void) const
+ {
+ return kmodel_;
+ }
+
+ /** Return the instance of the parsed robot description */
+ const boost::shared_ptr<robot_desc::URDF> &getParsedDescription(void) const
+ {
+ return urdf_;
+ }
+
+ const std::map< std::string, std::vector<std::string> > &getPlanningGroups(void) const
+ {
+ return planning_groups_;
+ }
+
+ std::vector< boost::shared_ptr<PlannerConfig> > getGroupPlannersConfig(const std::string &group);
+
+ /** Reload the robot description and recreate the model */
+ virtual void reload(void)
+ {
+ planning_groups_.clear();
+ kmodel_.reset();
+ urdf_.reset();
+ loadRobot();
+ }
+
+ protected:
+
+ void loadRobot(void);
+ void getPlanningGroups(std::map< std::string, std::vector<std::string> > &groups);
+
+ ros::NodeHandle nh_;
+
+ std::string description_;
+ std::map< std::string, std::vector<std::string> > planning_groups_;
+
+
+ boost::shared_ptr<planning_models::KinematicModel> kmodel_;
+ boost::shared_ptr<robot_desc::URDF> urdf_;
+ };
+
+
+}
+
+#endif
+
Property changes on: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
___________________________________________________________________
Added: svn:mergeinfo
+
Added: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b planning_environment is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,24 @@
+<package>
+ <description brief="planning_environment">
+
+ Define the robot model and collision environment based on ROS
+ parameters.
+
+ </description>
+ <author>Ioan Sucan</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+
+ <depend package="roscpp" />
+ <depend package="rosconsole" />
+ <depend package="pr2_defs" />
+ <depend package="planning_models" />
+ <depend package="collision_space" />
+
+ <url>http://pr.willowgarage.com/wiki/planning_environment</url>
+
+ <export>
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lplanning_environment" />
+ </export>
+
+</package>
Added: pkg/trunk/motion_planning/planning_environment/planning.yaml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/planning.yaml (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/planning.yaml 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,74 @@
+## This file should be loaded under <robot description>_planning
+
+## the list of groups for which motion planning can be performed
+group_list:
+ base
+ left_arm
+ right_arm
+ arms
+ base_left_arm
+ base_right_arm
+ base_arms
+
+## the definition of each group
+groups:
+
+ base:
+ links:
+ base_link
+ planner_configs:
+ RRTConfig1 SBLConfig1
+
+ left_arm:
+ links:
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ l_upper_arm_roll_link
+ l_upper_arm_link
+ l_elbow_flex_link
+ l_forearm_roll_link
+ l_forearm_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ planner_configs:
+ RRTConfig2 SBLConfig2
+
+ right_arm:
+ links:
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ r_upper_arm_roll_link
+ r_upper_arm_link
+ r_elbow_flex_link
+ r_forearm_roll_link
+ r_forearm_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ planner_configs:
+ RRTConfig2 SBLConfig2
+
+## the planner configurations; each config must have a type, which specifies
+## the planner to be used; other parameters can be specified as well
+
+planner_configs:
+
+ RRTConfig1:
+ type: RRT
+ range: 0.75
+
+ SBLConfig1:
+ type: SBL
+ projection: 0 1
+ celldim: 1 1
+ range: 0.5
+
+ RRTConfig2:
+ type: RRT
+ range: 0.75
+
+ SBLConfig2:
+ type: SBL
+ projection: 5 6
+ celldim: 0.1 0.1
+
+
\ No newline at end of file
Property changes on: pkg/trunk/motion_planning/planning_environment/planning.yaml
___________________________________________________________________
Added: svn:mergeinfo
+
Added: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,126 @@
+/*********************************************************************
+* 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_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())
+ {
+ getCollisionCheckLinks(collision_check_links_);
+ getSelfCollisionGroups(self_collision_check_groups_);
+ getSelfSeeLinks(self_see_links_);
+
+ ode_collision_model_->lock();
+ unsigned int cid = ode_collision_model_->addRobotModel(kmodel_, collision_check_links_, scale_, padd_);
+ for (unsigned int i = 0 ; i < self_collision_check_groups_.size() ; ++i)
+ ode_collision_model_->addSelfCollisionGroup(cid, self_collision_check_groups_[i]);
+ ode_collision_model_->updateRobotModel(cid);
+ ode_collision_model_->unlock();
+ }
+}
+
+void planning_environment::CollisionModels::getSelfSeeLinks(std::vector<std::string> &links)
+{
+ std::string link_list;
+ nh_.param(description_ + "_collision/self_see", link_list, std::string(""));
+ std::stringstream link_list_stream(link_list);
+
+ while (link_list_stream.good() && !link_list_stream.eof())
+ {
+ std::string name;
+ link_list_stream >> name;
+ if (name.size() == 0)
+ continue;
+ if (urdf_->getLink(name))
+ links.push_back(name);
+ else
+ ROS_ERROR("Unknown link: '%s'", name.c_str());
+ }
+}
+
+void planning_environment::CollisionModels::getCollisionCheckLinks(std::vector<std::string> &links)
+{
+ std::string link_list;
+ nh_.param(description_ + "_collision/collision_links", link_list, std::string(""));
+ std::stringstream link_list_stream(link_list);
+
+ while (link_list_stream.good() && !link_list_stream.eof())
+ {
+ std::string name;
+ link_list_stream >> name;
+ if (name.size() == 0)
+ continue;
+ if (urdf_->getLink(name))
+ links.push_back(name);
+ else
+ ROS_ERROR("Unknown link: '%s'", name.c_str());
+ }
+}
+
+void planning_environment::CollisionModels::getSelfCollisionGroups(std::vector< std::vector<std::string> > &groups)
+{
+ std::string group_list;
+ nh_.param(description_ + "_collision/self_collision_groups", group_list, std::string(""));
+ std::stringstream group_list_stream(group_list);
+ while (group_list_stream.good() && !group_list_stream.eof())
+ {
+ std::string name;
+ std::string group_elems;
+ group_list_stream >> name;
+ if (name.size() == 0)
+ continue;
+ nh_.param(description_ + "_collision/" + name, group_elems, std::string(""));
+ std::stringstream group_elems_stream(group_elems);
+ std::vector<std::string> this_group;
+ while (group_elems_stream.good() && !group_elems_stream.eof())
+ {
+ std::string link_name;
+ group_elems_stream >> link_name;
+ if (link_name.size() == 0)
+ continue;
+ if (urdf_->getLink(link_name))
+ this_group.push_back(link_name);
+ else
+ ROS_ERROR("Unknown link: '%s'", link_name.c_str());
+ }
+ if (this_group.size() > 0)
+ groups.push_back(this_group);
+ }
+}
Added: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,127 @@
+/*********************************************************************
+* 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/robot_models.h"
+#include <ros/console.h>
+#include <sstream>
+
+void planning_environment::RobotModels::loadRobot(void)
+{
+ std::string content;
+ if (nh_.getParam(description_, content))
+ {
+ urdf_ = boost::shared_ptr<robot_desc::URDF>(new robot_desc::URDF());
+ if (urdf_->loadString(content.c_str()))
+ {
+ getPlanningGroups(planning_groups_);
+ kmodel_ = boost::shared_ptr<planning_models::KinematicModel>(new planning_models::KinematicModel());
+ kmodel_->setVerbose(false);
+ kmodel_->build(*urdf_, planning_groups_);
+
+
+ // make sure the kinematic model is in its own frame
+ // (remove all transforms caused by planar or floating
+ // joints)
+ kmodel_->reduceToRobotFrame();
+ kmodel_->defaultState();
+ }
+ else
+ urdf_.reset();
+ }
+ else
+ ROS_ERROR("Robot model '%s' not found!", description_.c_str());
+}
+
+
+void planning_environment::RobotModels::getPlanningGroups(std::map< std::string, std::vector<std::string> > &groups)
+{
+ std::string group_list;
+ nh_.param(description_ + "_planning/group_list", group_list, std::string(""));
+ std::stringstream group_list_stream(group_list);
+ while (group_list_stream.good() && !group_list_stream.eof())
+ {
+ std::string name;
+ std::string group_elems;
+ group_list_stream >> name;
+ if (name.size() == 0)
+ continue;
+ nh_.param(description_ + "_planning/groups/" + name + "/links", group_elems, std::string(""));
+ std::stringstream group_elems_stream(group_elems);
+ while (group_elems_stream.good() && !group_elems_stream.eof())
+ {
+ std::string link_name;
+ group_elems_stream >> link_name;
+ if (link_name.size() == 0)
+ continue;
+ if (urdf_->getLink(link_name))
+ groups[name].push_back(link_name);
+ else
+ ROS_ERROR("Unknown link: '%s'", link_name.c_str());
+ }
+ }
+}
+
+bool planning_environment::RobotModels::PlannerConfig::hasParam(const std::string ¶m)
+{
+ return nh_.hasParam(description_ + "_planning/planner_configs/" + config_ + "/" + param);
+}
+
+std::string planning_environment::RobotModels::PlannerConfig::getParam(const std::string ¶m)
+{
+ std::string value;
+ nh_.param(description_ + "_planning/planner_configs/" + config_ + "/" + param, value, std::string(""));
+ return value;
+}
+
+std::vector< boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> > planning_environment::RobotModels::getGroupPlannersConfig(const std::string &group)
+{
+ std::string configs_list;
+ nh_.param(description_ + "_planning/groups/" + group + "/planner_configs", configs_list, std::string(""));
+
+ std::vector< boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> > configs;
+
+ std::stringstream configs_stream(configs_list);
+ while (configs_stream.good() && !configs_stream.eof())
+ {
+ std::string cfg;
+ configs_stream >> cfg;
+ if (cfg.size() == 0)
+ continue;
+ if (nh_.hasParam(description_ + "_planning/planner_configs/" + cfg + "/type"))
+ configs.push_back(boost::shared_ptr<PlannerConfig>(new PlannerConfig(description_, cfg)));
+ }
+ return configs;
+}
Property changes on: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
___________________________________________________________________
Added: svn:mergeinfo
+
Added: pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,56 @@
+/*********************************************************************
+* 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/robot_models.h>
+#include <gtest/gtest.h>
+#include <iostream>
+#include <sstream>
+
+TEST(Loading, EmptyRobot)
+{
+ planning_environment::RobotModels m("robotdesc/pr2");
+
+ EXPECT_TRUE(m.getKinematicModel().get() != NULL);
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "test_robot_models");
+
+ return RUN_ALL_TESTS();
+}
Added: pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml 2009-06-05 01:17:32 UTC (rev 16750)
@@ -0,0 +1,8 @@
+<launch>
+
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
+ <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find planning_environment)/planning.yaml" />
+
+ <test test-name="test_robot_models" pkg="planning_environment" type="test_robot_models" />
+
+</launch>
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-05 01:17:32 UTC (rev 16750)
@@ -3,7 +3,7 @@
rospack(planning_models)
rospack_add_boost_directories()
-#set(CMAKE_BUILD_TYPE Release)
+set(ROS_BUILD_TYPE Debug)
rospack_add_library(planning_models src/kinematic.cpp
src/load_mesh.cpp)
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-05 01:17:32 UTC (rev 16750)
@@ -46,6 +46,7 @@
#include <iostream>
#include <vector>
#include <string>
+#include <map>
#include <cassert>
/** @htmlinclude ../../manifest.html
@@ -569,8 +570,8 @@
delete m_robots[i];
}
- void build(const std::string &description, bool ignoreSensors = false);
- virtual void build(const robot_desc::URDF &model, bool ignoreSensors = false);
+ void build(const std::string &description, const std::map< std::string, std::vector<std::string> > &groups, bool ignoreSensors = false);
+ virtual void build(const robot_desc::URDF &model, const std::map< std::string, std::vector<std::string> > &groups, bool ignoreSensors = false);
bool isBuilt(void) const;
StateParams* newStateParams(void);
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-05 01:17:32 UTC (rev 16750)
@@ -384,15 +384,15 @@
return result;
}
-void planning_models::KinematicModel::build(const std::string &description, bool ignoreSensors)
+void planning_models::KinematicModel::build(const std::string &description, const std::map< std::string, std::vector<std::string> > &groups, bool ignoreSensors)
{
robot_desc::URDF *file = new robot_desc::URDF();
file->loadString(description.c_str());
- build(*file, ignoreSensors);
+ build(*file, groups, ignoreSensors);
delete file;
}
-void planning_models::KinematicModel::build(const robot_desc::URDF &model, bool ignoreSensors)
+void planning_models::KinematicModel::build(const robot_desc::URDF &model, const std::map< std::string, std::vector<std::string> > &groups, bool ignoreSensors)
{
if (m_built)
{
Modified: pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp 2009-06-05 01:17:32 UTC (rev 16750)
@@ -66,10 +66,10 @@
"<?xml version=\"1.0\" ?>"
"<robot name=\"myrobot\">"
"</robot>";
-
+ std::map < std::string, std::vector<std::string> > groups;
planning_models::KinematicModel *model = new planning_models::KinematicModel();
model->setVerbose(false);
- model->build(MODEL0);
+ model->build(MODEL0, groups);
EXPECT_TRUE(model->isBuilt());
EXPECT_TRUE(model->reduceToRobotFrame());
@@ -87,9 +87,9 @@
model->getJoints(joints);
EXPECT_EQ((unsigned int)0, joints.size());
- std::vector<std::string> groups;
- model->getGroups(groups);
- EXPECT_EQ((unsigned int)0, groups.size());
+ std::vector<std::string> pgroups;
+ model->getGroups(pgroups);
+ EXPECT_EQ((unsigned int)0, pgroups.size());
delete model;
}
@@ -198,10 +198,12 @@
"The state components for this group are: 0 1 2 base_link_joint base_link_joint base_link_joint \n";
planning_models::KinematicModel *model = new planning_models::KinematicModel();
+ std::map < std::string, std::vector<std::string> > groups;
+ groups["base"].push_back("base_link");
model->setVerbose(false);
robot_desc::URDF *file = new robot_desc::URDF();
file->loadString(MODEL1.c_str());
- model->build(*file);
+ model->build(*file, groups);
EXPECT_TRUE(model->isBuilt());
EXPECT_TRUE(model->reduceToRobotFrame());
@@ -363,10 +365,14 @@
"Available groups: one_robot::base \n"
"Group one_robot::base with ID 0 has 1 roots: base_link_joint \n"
"The state components for this group are: 0 1 2 3 4 base_link_joint base_link_joint base_link_joint link_a_joint link_c_joint \n";
-
+ std::map < std::string, std::vector<std::string> > groups;
+ groups["base"].push_back("base_link");
+ groups["base"].push_back("link_a");
+ groups["base"].push_back("link_b");
+ groups["base"].push_back("link_c");
planning_models::KinematicModel *model = new planning_models::KinematicModel();
model->setVerbose(false);
- model->build(MODEL2);
+ model->build(MODEL2, groups);
EXPECT_TRUE(model->isBuilt());
EXPECT_TRUE(model->reduceToRobotFrame());
@@ -731,10 +737,34 @@
"The state components for this group are: 0 1 2 3 4 5 6 7 8 9 base_link1_joint base_link1_joint base_link1_joint link_a_joint link_c_joint base_link2_joint base_link2_joint base_link2_joint link_d_joint link_f_joint \n"
"Group more_robots::r2 with ID 3 has 1 roots: base_link2_joint \n"
"The state components for this group are: 5 6 7 8 9 base_link2_joint base_link2_joint base_link2_joint link_d_joint link_f_joint \n";
-
+
+ std::map < std::string, std::vector<std::string> > groups;
+ groups["r1"].push_back("base_link1");
+ groups["r1"].push_back("link_a");
+ groups["r1"].push_back("link_b");
+ groups["r1"].push_back("link_c");
+ groups["r2"].push_back("base_link2");
+ groups["r2"].push_back("link_d");
+ groups["r2"].push_back("link_e");
+ groups["r2"].push_back("link_f");
+ groups["r1r2"].push_back("base_link1");
+ groups["r1r2"].push_back("link_a");
+ groups["r1r2"].push_back("link_b");
+ groups["r1r2"].push_back("link_c");
+ groups["r1r2"].push_back("base_link2");
+ groups["r1r2"].push_back("link_d");
+ groups["r1r2"].push_back("link_e");
+ groups["r1r2"].push_back("link_f");
+ groups["parts"].push_back("base_link1");
+ groups["parts"].push_back("link_a");
+ groups["parts"].push_back("link_b");
+ groups["parts"].push_back("link_e");
+ groups["parts"].push_back("link_f");
+ groups["parts"].push_back("base_link3");
+
planning_models::KinematicModel *model = new planning_models::KinematicModel();
model->setVerbose(false);
- model->build(MODEL3);
+ model->build(MODEL3, groups);
EXPECT_TRUE(model->isBuilt());
EXPECT_TRUE(model->reduceToRobotFrame());
Modified: pkg/trunk/util/self_watch/manifest.xml
===================================================================
--- pkg/trunk/util/self_watch/manifest.xml 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/util/self_watch/manifest.xml 2009-06-05 01:17:32 UTC (rev 16750)
@@ -7,9 +7,7 @@
<depend package="rosconsole" />
<depend package="robot_msgs" />
+ <depend package="planning_environment" />
- <depend package="wg_robot_description_parser" />
- <depend package="planning_models" />
- <depend package="collision_space" />
</package>
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-06-05 00:23:35 UTC (rev 16749)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-06-05 01:17:32 UTC (rev 16750)
@@ -36,7 +36,7 @@
#include <ros/ros.h>
-#include <collision_space/environmentODE.h>
+#include <planning_environment/collision_models.h>
#include <robot_msgs/MechanismState.h>
class SelfWatch
@@ -62,84 +62,53 @@
m_nodeHandle.param("self_collision_scale_factor", m_scaling, 1.2);
m_nodeHandle.param("self_collision_padding", m_padding, 0.05);
- // load the string description of the robot
- std::string content;
- if (m_nodeHandle.getParam("robot_description", content))
+ if (!m_nodeHandle.hasParam("robot_description"))
+ ROS_ERROR("No robot description found");
+ else
{
- // parse the description
- robot_desc::URDF *file = new robot_desc::URDF();
- if (file->loadString(content.c_str()))
+ m_envModels = boost::shared_ptr<planning_environment::CollisionModels>(new planning_environment::CollisionModels("robot_description", m_scaling, m_padding));
+ m_kmodel = m_envModels->getKinematicModel();
+ m_collisionSpace = m_envModels->getODECollisionModel();
+
+ // create a state that can be used to monitor the
+ // changes in the joints of the kinematic model
+ m_robotState = boost::shared_ptr<planning_models::KinematicModel::StateParams>(m_kmodel->newStateParams());
+
+ // make sure the transforms caused by the planar and
+ // floating joints are identity, to be compatible with
+ // the fact we are considering the robot in its own
+ // frame
+ m_robotState->setInRobotFrame();
+
+ // get the list of links that are enabled for collision checking
+ std::vector<std::string> links = m_envModels->getCollisionCheckLinks();
+
+ // print some info, just to easily double-check the loaded data
+ if (links.empty())
+ ROS_WARN("No links have been enabled for collision checking");
+ else
{
- // create a kinematic model out of the parsed description
- m_kmodel = boost::shared_ptr<planning_models::KinematicModel>(new planning_models::KinematicModel());
- m_kmodel->setVerbose(false);
- m_kmodel->build(*file);
-
- // make sure the kinematic model is in its own frame
- // (remove all transforms caused by planar or floating
- // joints)
- m_kmodel->reduceToRobotFrame();
-
- // create a state that can be used to monitor the
- // changes in the joints of the kinematic model
- m_robotState = boost::shared_ptr<planning_models::KinematicModel::StateParams>(m_kmodel->newStateParams());
-
- // make sure the transforms caused by the planar and
- // floating joints are identity, to be compatible with
- // the fact we are considering the robot in its own
- // frame
- m_robotState->setInRobotFrame();
-
- // create a new collision space
- m_collisionSpace = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelODE());
-
- // enable self collision checking (just in case default is disabled)
- m_collisionSpace->setSelfCollision(true);
-
- // get the list of links that are enabled for collision checking
- std::vector<std::string> links;
- robot_desc::URDF::Group *g = file->getGroup("collision_check");
- if (g && g->hasFlag("collision"))
- links = g->linkNames;
-
- // print some info, just to easily double-check the loaded data
- if (links.empty())
- ROS_WARN("No links have been enabled for collision checking");
- else
- {
- ROS_INFO("Collision checking enabled for links: ");
- for (unsigned int i = 0 ; i < links.size() ; ++i)
- ROS_INFO(" %s", links[i].c_str());
- }
-
- // add the robot model to the collision spac...
[truncated message content] |
