Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18675] pkg/trunk/motion_planning/ planning_environment
|
From: <is...@us...> - 2009-07-12 03:10:10
|
Revision: 18675
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18675&view=rev
Author: isucan
Date: 2009-07-12 03:10:08 +0000 (Sun, 12 Jul 2009)
Log Message:
-----------
added a state validity checker
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/src/view_state_validity.cpp
pkg/trunk/motion_planning/planning_environment/view_state_validity.launch
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-12 02:50:16 UTC (rev 18674)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-12 03:10:08 UTC (rev 18675)
@@ -14,10 +14,14 @@
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)
+target_link_libraries(view_state_validity planning_environment)
+
# Tests
# Create a model of the PR2
-rospack_add_executable(test_robot_models test/test_robot_models.cpp)
+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)
Copied: pkg/trunk/motion_planning/planning_environment/src/view_state_validity.cpp (from rev 18643, pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/view_state_validity.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/view_state_validity.cpp 2009-07-12 03:10:08 UTC (rev 18675)
@@ -0,0 +1,122 @@
+/*********************************************************************
+* 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 */
+
+/**
+
+@b ViewStateValidity is a node capable of verifying if the current
+state of the robot is valid or not.
+
+**/
+
+#include "planning_environment/collision_space_monitor.h"
+#include <std_msgs/Byte.h>
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <map>
+
+class ViewStateValidity
+{
+public:
+
+ ViewStateValidity(void) : last_(-1)
+ {
+ collisionModels_ = new planning_environment::CollisionModels("robot_description");
+ if (collisionModels_->loadedModels())
+ {
+ stateValidityPublisher_ = nh_.advertise<std_msgs::Byte>("state_validity", 1);
+ collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, &tf_);
+ collisionSpaceMonitor_->getEnvironmentModel()->setVerbose(true);
+ collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&ViewStateValidity::afterWorldUpdate, this, _1));
+ collisionSpaceMonitor_->setOnStateUpdateCallback(boost::bind(&ViewStateValidity::stateUpdate, this));
+ }
+ }
+
+ virtual ~ViewStateValidity(void)
+ {
+ if (collisionSpaceMonitor_)
+ delete collisionSpaceMonitor_;
+ if (collisionModels_)
+ delete collisionModels_;
+ }
+
+protected:
+
+ void afterWorldUpdate(const mapping_msgs::CollisionMapConstPtr &collisionMap)
+ {
+ last_ = -1;
+ }
+
+ void stateUpdate(void)
+ {
+ collisionSpaceMonitor_->getEnvironmentModel()->lock();
+ collisionSpaceMonitor_->getKinematicModel()->computeTransforms(collisionSpaceMonitor_->getRobotState()->getParams());
+ collisionSpaceMonitor_->getEnvironmentModel()->updateRobotModel();
+ bool invalid = collisionSpaceMonitor_->getEnvironmentModel()->isCollision();
+ collisionSpaceMonitor_->getEnvironmentModel()->unlock();
+ std_msgs::Byte msg;
+ msg.data = invalid ? 0 : 1;
+ if (last_ != msg.data)
+ {
+ last_ = msg.data;
+ stateValidityPublisher_.publish(msg);
+ if (invalid)
+ ROS_WARN("State is in collision");
+ else
+ ROS_INFO("State is valid");
+ }
+ }
+
+private:
+
+ int last_;
+ ros::NodeHandle nh_;
+ ros::Publisher stateValidityPublisher_;
+ tf::TransformListener tf_;
+ planning_environment::CollisionModels *collisionModels_;
+ planning_environment::CollisionSpaceMonitor *collisionSpaceMonitor_;
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "view_state_validity");
+
+ ViewStateValidity validator;
+ ros::spin();
+
+ return 0;
+}
Property changes on: pkg/trunk/motion_planning/planning_environment/src/view_state_validity.cpp
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/motion_planning/ompl_planning/src/state_validity_monitor.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Added: pkg/trunk/motion_planning/planning_environment/view_state_validity.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/view_state_validity.launch (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/view_state_validity.launch 2009-07-12 03:10:08 UTC (rev 18675)
@@ -0,0 +1,12 @@
+
+<launch>
+ <node pkg="planning_environment" type="view_state_validity" respawn="false" output="screen">
+ <remap from="robot_description" to="robotdesc/pr2" />
+ <remap from="collision_map" to="collision_map_occ" />
+ <remap from="collision_map_update" to="collision_map_occ_update" />
+ <param name="refresh_interval_collision_map" type="double" value="5.0" />
+ <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
+ <param name="refresh_interval_pose" type="double" value="1.0" />
+ <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
+ </node>
+</launch>
Property changes on: pkg/trunk/motion_planning/planning_environment/view_state_validity.launch
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
