[Personalrobots-commit] SF.net SVN: personalrobots:[16761] pkg/trunk/motion_planning/ planning_environment

[<is...@us...>]

Revision: 16761
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16761&view=rev
Author:   isucan
Date:     2009-06-05 06:14:48 +0000 (Fri, 05 Jun 2009)

Log Message:
-----------
moved some code

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/collision_checks.yaml
    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/src/collision_models.cpp
    pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp

Modified: pkg/trunk/motion_planning/planning_environment/collision_checks.yaml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/collision_checks.yaml	2009-06-05 04:34:04 UTC (rev 16760)
+++ pkg/trunk/motion_planning/planning_environment/collision_checks.yaml	2009-06-05 06:14:48 UTC (rev 16761)
@@ -74,3 +74,9 @@
   torso_lift_link
   base_link
   
+
+## The padding to be added for the body parts the robot can see
+self_see_padd: 0.1
+
+## The scaling to be considered for the body parts the robot can see
+self_see_scale: 1.0

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h	2009-06-05 04:34:04 UTC (rev 16760)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h	2009-06-05 06:14:48 UTC (rev 16761)
@@ -69,9 +69,6 @@
 	{
 	    RobotModels::reload();
 	    ode_collision_model_.reset();
-	    self_see_links_.clear();
-	    collision_check_links_.clear();
-	    self_collision_check_groups_.clear();
 	    loadCollision();
 	}
 	
@@ -81,32 +78,10 @@
 	    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_;

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h	2009-06-05 04:34:04 UTC (rev 16760)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h	2009-06-05 06:14:48 UTC (rev 16761)
@@ -112,14 +112,35 @@
 	    return planning_groups_;
 	}	
 	
+	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_;
+	}
+	
+	double getSelfSeePadding(void);
+	double getSelfSeeScale(void);
+	
 	std::vector< boost::shared_ptr<PlannerConfig> > getGroupPlannersConfig(const std::string &group);
 	
 	/** Reload the robot description and recreate the model */
 	virtual void reload(void)
 	{
-	    planning_groups_.clear();
 	    kmodel_.reset();
 	    urdf_.reset();
+	    planning_groups_.clear();
+	    self_see_links_.clear();
+	    collision_check_links_.clear();
+	    self_collision_check_groups_.clear();
 	    loadRobot();
 	}
 	
@@ -128,14 +149,23 @@
 	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::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_;
+	
+	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::vector<std::string> >            self_collision_check_groups_;
+
     };
     
 	

Modified: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp	2009-06-05 04:34:04 UTC (rev 16760)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp	2009-06-05 06:14:48 UTC (rev 16761)
@@ -55,72 +55,3 @@
 	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);
-    }
-}

Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp	2009-06-05 04:34:04 UTC (rev 16760)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp	2009-06-05 06:14:48 UTC (rev 16761)
@@ -125,3 +125,86 @@
     }
     return configs;
 }
+
+void planning_environment::RobotModels::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::RobotModels::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::RobotModels::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);
+    }
+}
+
+double planning_environment::RobotModels::getSelfSeePadding(void)
+{
+    double value;
+    nh_.param(description_ + "_collision/self_see_padd", value, 0.0);
+    return value;
+}
+
+double planning_environment::RobotModels::getSelfSeeScale(void)
+{
+    double value;
+    nh_.param(description_ + "_collision/self_see_scale", value, 1.0);
+    return value;
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.